#include "StdAfx.h"
#include <CryHeaders.h>

#include "Model.h"
#include "CharacterInstance.h"
#include "CharacterManager.h"

///////////////////////////////////////////// physics stuff /////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////


int CSkeletonPose::getBonePhysParentOrSelfIndex(int iBoneIndex, int nLod)
{
	int iNextBoneIndex;
	for( ;!GetModelJointPointer(iBoneIndex)->m_PhysInfo[nLod].pPhysGeom; iBoneIndex=iNextBoneIndex)
		if ((iNextBoneIndex=getBoneParentIndex(iBoneIndex))==iBoneIndex)
			return -1;
	return iBoneIndex;
}

//////////////////////////////////////////////////////////////////////////
// finds the first physicalized child (or itself) of the given bone (bone given by index)
// returns -1 if it's not physicalized
int CSkeletonPose::getBonePhysChildIndex (int iBoneIndex, int nLod)
{
	CModelJoint* pBoneInfo = GetModelJointPointer(iBoneIndex);
	if (pBoneInfo->m_PhysInfo[nLod].pPhysGeom)
		return iBoneIndex;
	unsigned numChildren = pBoneInfo->numChildren();
	unsigned nFirstChild = pBoneInfo->getFirstChildIndexOffset() + iBoneIndex;
	if (nFirstChild==iBoneIndex)
		return -1;
	for (unsigned nChild = 0; nChild < numChildren; ++nChild)
	{
		int nResult = getBonePhysChildIndex(nFirstChild + nChild, nLod);
		if (nResult >= 0)
			return nResult;
	}
	return -1;
}

int CSkeletonPose::GetBoneSurfaceTypeId(int nBoneIndex, int nLod)
{
	phys_geometry *pGeom = GetModelJointPointer(nBoneIndex)->m_PhysInfo[nLod].pPhysGeom;
	if (pGeom)
		return pGeom->pMatMapping[pGeom->surface_idx];
	else
		return -1;
}

int CSkeletonPose::TranslatePartIdToDeadBody(int partid)
{
	if ((unsigned int)partid>=(unsigned int)GetJointCount())
		return -1;

	int nLod = GetPhysicsLod();
	if (GetModelJointPointer(partid)->m_PhysInfo[nLod].pPhysGeom)
		return partid;

	return getBonePhysParentIndex(partid,nLod);
}

void CSkeletonPose::BuildPhysicalEntity(IPhysicalEntity *pent,float mass,int surface_idx,float stiffness_scale, 
																		int nLod,int partid0, const Matrix34 &mtxloc)
{
	float scaleOrg=mtxloc.GetColumn(0).GetLength(), scale=scaleOrg/m_pInstance->m_RenderCharLocation.s;
	int nLodOrg = nLod;
	nLod = min(nLod, (int)m_pInstance->m_pModel->GetModelMeshCount()-1);
	
  //scale = m_pInstance->GetUniformScale();

  Vec3 offset=mtxloc.GetTranslation(); 
	if (fabs_tpl(scale-1.0f)<0.01f)
		scale = 1.0f;

	if (m_pInstance->m_pModel->m_ObjectType==CGA)
	{
		assert( pent );

		m_pCharPhysics = pent;

		uint32 i;
		float totalVolume = 0;

		uint32 numJoints = m_arrCGAJoints.size();
		assert(numJoints);
		for (i=0; i <numJoints; i++)
		{
			CCGAJoint *joint = &m_arrCGAJoints[i];
			if (!joint->m_CGAObjectInstance)
				continue;
			phys_geometry *geom = joint->m_CGAObjectInstance->GetPhysGeom();
			if (geom)
			{
				totalVolume += geom->V;
			}
		}
		float density = totalVolume>FLT_EPSILON ? mass/totalVolume:0.0f;

		pe_articgeomparams params;
		//	uint32 numJoints = m_arrJoints.size();
		//	assert(numJoints);
		for (i = 0; i <numJoints ; i++)
		{
			CCGAJoint *joint = &m_arrCGAJoints[i];
			//SJoint *sjoint = &m_arrJoints[i];
			m_parrModelJoints[i].m_NodeID = ~0;
			if (!joint->m_CGAObjectInstance)
				continue;

			IStatObj::SSubObject *pSubObj;
			const char *pstrMass;
			if (m_pInstance->m_pModel->pCGA_Object && (pSubObj=m_pInstance->m_pModel->pCGA_Object->GetSubObject(i)) && 
				(pstrMass=strstr(pSubObj->properties,"mass")))
			{
				params.density = 0;
				for(; *pstrMass && !isdigit((unsigned char)(*pstrMass)); pstrMass++);
				params.mass = (float)atof(pstrMass);
			}	
			else
			{
				params.mass = 0;
				params.density = density;
			}

			// Add collision geometry.
			params.flags = geom_collides|geom_floats;
			phys_geometry *geom = joint->m_CGAObjectInstance->GetPhysGeom(PHYS_GEOM_TYPE_NO_COLLIDE),
				*geomProxy = joint->m_CGAObjectInstance->GetPhysGeom();
			if (geom && !geomProxy)
				params.flags = geom_colltype_ray;
			if (!geom)
				geom=geomProxy, geomProxy=0;
			params.pos = GetPoseData().m_pJointsAbsolute[i].t*scale;
			params.q = GetPoseData().m_pJointsAbsolute[i].q;
			params.scale = scaleOrg;
			params.idbody = i;
			if (geom)
			{
				m_parrModelJoints[i].m_NodeID = m_pCharPhysics->AddGeometry( geom, &params, partid0+i );
				//joint->m_Physics = true;
				joint->m_qqqhasPhysics = partid0+i;
			}
			if (geomProxy)
			{
				params.flags |= geom_proxy;
				m_pCharPhysics->AddGeometry( geomProxy, &params, partid0+i );
			}
		}

		pe_params_joint pj;
		pj.op[0] = -1;
		pj.flagsPivot = 7;
		if (pent->GetType()==PE_ARTICULATED) for(i=0; i<numJoints; i++) 
			if (m_arrCGAJoints[i].m_CGAObjectInstance && m_arrCGAJoints[i].m_CGAObjectInstance->GetPhysGeom())
			{
				pj.op[1] = i;
				pj.pivot = GetPoseData().m_pJointsAbsolute[i].t;
				pent->SetParams(&pj);
			}


		int j,numSlots= (m_pInstance->m_pModel->pCGA_Object) ? m_pInstance->m_pModel->pCGA_Object->GetSubObjectCount() : 0;
		IStatObj::SSubObject *pSubObj;
		for (i=0; i<numJoints; i++)
			if (m_arrCGAJoints[i].m_CGAObjectInstance && m_arrCGAJoints[i].m_CGAObjectInstance->GetPhysGeom())
				for(j=0; j<min(numSlots,(int)numJoints); j++) 
					if ((pSubObj=m_pInstance->m_pModel->pCGA_Object->GetSubObject(j)) && pSubObj->pStatObj==m_arrCGAJoints[i].m_CGAObjectInstance) {
						m_arrCGAJoints[j].m_qqqhasPhysics = i; break;
					}

		if (m_pInstance->m_pModel->pCGA_Object)
		{
			m_pInstance->m_pModel->pCGA_Object->PhysicalizeSubobjects(m_pCharPhysics,&mtxloc,mass,density,partid0, 
				strided_pointer<int>(&m_arrCGAJoints[0].m_qqqhasPhysics,sizeof(m_arrCGAJoints[0])));
		}

		for (i=0; i<numJoints; i++)
			m_arrCGAJoints[i].m_qqqhasPhysics = m_parrModelJoints[i].m_NodeID;	
	}
	else
	{

		pe_type pentype = pent->GetType();
		int i,j;
		pe_geomparams gp;
		pe_articgeomparams agp;
		pe_params_articulated_body pab;
		pe_geomparams *pgp = pentype==PE_ARTICULATED ? &agp:&gp;
		pgp->flags = pentype==PE_LIVING ? 0:geom_collides|geom_floats;
		pgp->bRecalcBBox = 0;
		float volume[2]={0,0},M=0,density,k;

		for (i=pab.nJointsAlloc=0; i<(int)GetJointCount(); ++i)
			if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom)
			{
				volume[GetModelJointPointer(i)->m_fMass>0.0f] += GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom->V*cube(scale);
				M += GetModelJointPointer(i)->m_fMass;
				pab.nJointsAlloc++;
			}
		density = volume[1]>0.0f ? M/volume[1] : 1.0f;
		k = M+volume[0]>0.0f ? mass/(M+volume[0]*density) : 1.0f;

		pgp->scale = scaleOrg;

		if (surface_idx>=0)
			pgp->surface_idx = surface_idx;

		pe_action_remove_all_parts tmp;
		pent->Action(&tmp);
		pent->SetParams(&pab);

		for(i=0;i<(int)GetJointCount();i++) 
			if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom) 
			{
				m_parrModelJoints[i].m_NodeID =~0;
				pgp->pos = GetPoseData().m_pJointsAbsolute[i].t*scale+offset;
				pgp->q = GetPoseData().m_pJointsAbsolute[i].q;
				pgp->flags = /*strstr(GetModelJointIdx(i)->m_strJointName,"Hand") ? geom_no_raytrace :*/ geom_collides|geom_floats;
				if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom->pForeignData)
					pgp->flags &= ~geom_colltype6;
				/*
        if( m_pInstance->m_bCustomScale[i] )
        {
          pgp->scale = m_pInstance->m_fCustomScale[i];
        }
        else 
        */
        if( !m_pInstance->m_IgnoreScale[i] )
        {
          pgp->scale = scaleOrg;
        }
        else
        {
          pgp->scale = 1.0f;
        }
				agp.idbody = i;
				while((j=getBonePhysParentIndex(agp.idbody,nLod))>=0 && 
							(GetModelJointPointer(agp.idbody)->m_PhysInfo[nLod].flags & all_angles_locked)==all_angles_locked)
					agp.idbody = j;
				if (!(pgp->mass = GetModelJointPointer(i)->m_fMass))
					pgp->mass = GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom->V*density;
				pgp->mass *= k;
				m_parrModelJoints[i].m_NodeID = pent->AddGeometry(GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom,pgp,i);
				if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom->pForeignData)
				{
					pgp->mass=0.0f; pgp->flags=geom_colltype6;
					pent->AddGeometry((phys_geometry*)GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom->pForeignData,pgp,i+200);
				}
			} else
				m_parrModelJoints[i].m_NodeID =~0;

			if (pentype==PE_ARTICULATED) 
			{
				//SJoint *pBone;
				CModelJoint* pBoneInfo;
				for(i=0;i<(int)GetJointCount();i++) 
					if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom)	
					{
						pe_params_joint pj;
						int iParts[16];
						//pBone = &m_arrJoints[i];
						pBoneInfo = GetModelJointPointer(i);

						pj.pSelfCollidingParts = iParts;
						if ((pj.flags = pBoneInfo->m_PhysInfo[nLod].flags)!=~0)
							pj.flags |= angle0_auto_kd*7;
						else pj.flags = angle0_locked;
						if (nLod)
							pj.flags &= ~(angle0_auto_kd*7);
						int iParent0;
						if ((iParent0=pj.op[0] = getBonePhysParentIndex(i,nLod))>=0)
						while((j=getBonePhysParentIndex(pj.op[0],nLod))>=0 && 
							(GetModelJointPointer(pj.op[0])->m_PhysInfo[nLod].flags & all_angles_locked)==all_angles_locked)
							pj.op[0] = j;
						if ((pj.flags & all_angles_locked)==all_angles_locked && pj.op[0]>=0)
							continue;
						pj.op[1] = i;

						pj.pivot = GetPoseData().m_pJointsAbsolute[i].t;

						pj.pivot = pj.pivot*scale+offset;

						pj.nSelfCollidingParts = 0;
						if ((/*ptr=*/strstr(pBoneInfo->GetJointName(),"Forearm"))) 
						{
							for(j=0; j<(int)GetJointCount() && pj.nSelfCollidingParts<sizeof(iParts)/sizeof(iParts[0]); j++) 
								if (GetModelJointPointer(j)->m_PhysInfo[nLod].pPhysGeom && 
									(strstr(GetModelJointPointer(j)->GetJointName(),"Pelvis") || strstr(GetModelJointPointer(j)->GetJointName(),"Head") || 
									strstr(GetModelJointPointer(j)->GetJointName(),"Spine") ||	
									(/*ptr1=*/strstr(GetModelJointPointer(j)->GetJointName(),"Thigh")) ||//&& ptr[-2]==ptr1[-2] || 
									strstr(GetModelJointPointer(j)->GetJointName(),"Forearm") && i>j))
									pj.pSelfCollidingParts[pj.nSelfCollidingParts++] = j;
						}	
						else 
							if ((/*ptr=*/strstr(pBoneInfo->GetJointName(),"Calf"))) 
							{
								for(j=0; j<(int)GetJointCount() && pj.nSelfCollidingParts<sizeof(iParts)/sizeof(iParts[0]); j++) 
									if (GetModelJointPointer(j)->m_PhysInfo[nLod].pPhysGeom && strstr(GetModelJointPointer(j)->GetJointName(),"Calf") && i>j)
										pj.pSelfCollidingParts[pj.nSelfCollidingParts++] = j;
							}

							if (pBoneInfo->m_PhysInfo[nLod].flags!=-1) 
								for(j=0;j<3;j++) 
								{
									pj.limits[0][j] = pBoneInfo->m_PhysInfo[nLod].min[j];
									pj.limits[1][j] = pBoneInfo->m_PhysInfo[nLod].max[j];
									pj.bounciness[j] = 0;
									pj.ks[j] = pBoneInfo->m_PhysInfo[nLod].spring_tension[j]*stiffness_scale;
									pj.kd[j] = pBoneInfo->m_PhysInfo[nLod].damping[j];
									if (fabsf(pj.limits[0][j])<3) 
									{
										pj.qdashpot[j] = 0.3f; pj.kdashpot[j] = 40.0f-nLod*36.0f;
									} 
									else pj.qdashpot[j] = pj.kdashpot[j] = 0;
								} 
							else 
								for(j=0;j<3;j++) 
								{
									pj.limits[0][j]=-1E10f; pj.limits[1][j]=1E10f;
									pj.bounciness[j]=0; pj.ks[j]=0; pj.kd[j]=stiffness_scale;
								}
								if (strstr(GetModelJointPointer(i)->GetJointName(),"Thigh") || strstr(GetModelJointPointer(i)->GetJointName(),"Calf"))
									pj.flags |= joint_ignore_impulses;

								pj.pMtx0 = 0;
								Matrix33 mtx0;
								if (pBoneInfo->m_PhysInfo[nLod].framemtx[0][0]<10 && pBoneInfo->m_PhysInfo[nLod].flags!=-1)
								{
									mtx0 = *(alias_cast<Matrix33*>( (float*)&pBoneInfo->m_PhysInfo[nLod].framemtx[0] ));									
									pj.pMtx0 = &mtx0;
								}
								else
									mtx0 = Matrix33(IDENTITY);
								if (iParent0!=pj.op[0])
									pj.pMtx0 = &(mtx0 = Matrix33(!GetPoseData().m_pJointsAbsolute[pj.op[0]].q*GetPoseData().m_pJointsAbsolute[iParent0].q)*mtx0);
								pent->SetParams(&pj);
					}
			}

			//m_vOffset = offset;
			//m_fScale = scale;
			m_fMass = mass;
			m_iSurfaceIdx = surface_idx;
			m_bPhysicsRelinquished = (nLodOrg > 0);
	}

	for(int i=m_pInstance->m_AttachmentManager.GetAttachmentCount()-1; i>=0; i--) 
		m_pInstance->m_AttachmentManager.PhysicalizeAttachment(i,nLod,pent,offset);
}

IPhysicalEntity *CSkeletonPose::GetCharacterPhysics(const char *pRootBoneName) 
{	
	if (!pRootBoneName)
		return m_pCharPhysics; 

	for(int i=0;i<m_nAuxPhys;i++) 
	{
		if (!strnicmp(m_auxPhys[i].strName,pRootBoneName,m_auxPhys[i].nChars))
			return m_auxPhys[i].pPhysEnt;
	}

	return m_pCharPhysics;
}
IPhysicalEntity *CSkeletonPose::GetCharacterPhysics(int iAuxPhys) 
{ 
	if (iAuxPhys<0)
		return m_pCharPhysics;
	if (iAuxPhys>=m_nAuxPhys)
		return 0;

	return m_auxPhys[iAuxPhys].pPhysEnt;
}

void CSkeletonPose::DestroyCharacterPhysics(int iMode) 
{
	m_pSkeletonAnim->FinishAnimationComputations();

	int i;
	for(i=0;i<m_nAuxPhys;i++) {
		g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[i].pPhysEnt,iMode);
		if (iMode==0)
		{
			delete[] m_auxPhys[i].pauxBoneInfo;
			m_auxPhys[i].pauxBoneInfo = NULL;

			delete[] m_auxPhys[i].pVtx;
			m_auxPhys[i].pVtx = NULL;
		}
	}
	if (iMode==0)
		m_nAuxPhys = 0;

	if (m_pCharPhysics)
		g_pIPhysicalWorld->DestroyPhysicalEntity(m_pCharPhysics,iMode);
	if (iMode==0) {
		m_pCharPhysics = 0;
		m_bPhysicsRelinquished=m_bAliveRagdoll = 0; m_timeStandingUp = -1;
		if (m_ppBonePhysics)
		{
			for(i=0;i<(int)GetJointCount();i++) if (m_ppBonePhysics[i])
				m_ppBonePhysics[i]->Release(), g_pIPhysicalWorld->DestroyPhysicalEntity(m_ppBonePhysics[i]);
			delete[] m_ppBonePhysics;
			m_ppBonePhysics = 0;
		}
	}

	if (iMode==2) for(i=0;i<m_nAuxPhys;i++) if (m_auxPhys[i].pPhysEnt->GetType()==PE_ROPE) {
		pe_params_rope pr;
		pr.bTargetPoseActive = 0;
		if (m_auxPhys[i].bTied0)
			pr.pEntTiedTo[0] = m_pCharPhysics;
		if (m_auxPhys[i].bTied1)
			pr.pEntTiedTo[1] = m_pCharPhysics;
		m_auxPhys[i].pPhysEnt->SetParams(&pr);
	}	else
	{
		int j;
		pe_action_attach_points aap;
		aap.pEntity = m_pCharPhysics;
		aap.nPoints = 1;
		aap.piVtx = &j;
		for(j=0; j<m_auxPhys[i].nBones; ) {
			aap.partid = getBonePhysParentIndex(m_auxPhys[i].pauxBoneInfo[j].iBone,0);
			m_auxPhys[i].pPhysEnt->Action(&aap);
			for(j++; j<m_auxPhys[i].nBones && !strncmp(GetModelJointPointer(getBoneParentIndex(m_auxPhys[i].pauxBoneInfo[j].iBone))->GetJointName(),
					m_auxPhys[i].strName,m_auxPhys[i].nChars); j++);
		}
	}
}





IPhysicalEntity *CSkeletonPose::CreateCharacterPhysics(IPhysicalEntity *pHost, float mass,int surface_idx,float stiffness_scale, int nLod, 
																									 const Matrix34 &mtxloc)
{
	m_pSkeletonAnim->FinishAnimationComputations();

	float scale = mtxloc.GetColumn(0).GetLength();

  //scale = m_pInstance->GetUniformScale();
  
  Vec3 offset = mtxloc.GetTranslation();
	if (fabs_tpl(scale-1.0f)<0.01f)
		scale = 1.0f;
  //
  //
	if (m_pCharPhysics) 
	{
		int i;
		g_pIPhysicalWorld->DestroyPhysicalEntity(m_pCharPhysics);
		m_pCharPhysics = 0;
		m_bPhysicsRelinquished=m_bAliveRagdoll = 0;	m_timeStandingUp = -1;
		for(i=0;i<m_nAuxPhys;i++)
		{
			g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[i].pPhysEnt);

			delete[] m_auxPhys[i].pauxBoneInfo;
			m_auxPhys[i].pauxBoneInfo = NULL;

			delete[] m_auxPhys[i].pVtx;
			m_auxPhys[i].pVtx = NULL;
		}
		m_nAuxPhys=0;
		if (m_ppBonePhysics)
		{
			for(i=0;i<(int)GetJointCount();i++) if (m_ppBonePhysics[i])
				m_ppBonePhysics[i]->Release(), g_pIPhysicalWorld->DestroyPhysicalEntity(m_ppBonePhysics[i]);
			delete[] m_ppBonePhysics;
			m_ppBonePhysics = 0;
		}
	}

	if (m_bHasPhysics) 
	{
		pe_params_foreign_data pfd;
		pfd.pForeignData=0; pfd.iForeignData=0;
		if (pHost) 
			pHost->GetParams(&pfd);

		pe_params_pos pp;
		pp.iSimClass = 6;
		m_pCharPhysics = g_pIPhysicalWorld->CreatePhysicalEntity(PE_ARTICULATED,5.0f,&pp, pfd.pForeignData,pfd.iForeignData);

		pe_params_articulated_body pab;
		pab.bGrounded = 1;
		pab.scaleBounceResponse = 0.6f;
		pab.bAwake = 0;

		//IVO_x
		pab.pivot = Vec3( GetPoseData().m_pJointsAbsolute[ getBonePhysChildIndex(0) ].t );

		m_pCharPhysics->SetParams(&pab);

		BuildPhysicalEntity(m_pCharPhysics, mass,surface_idx,stiffness_scale, 0,0, mtxloc);

		pe_params_joint pj;
		pj.op[0] = -1;
		pj.op[1] = getBonePhysChildIndex(0);
		pj.flags = all_angles_locked | joint_no_gravity | joint_isolated_accelerations;
		m_pCharPhysics->SetParams(&pj);

		if (pHost) 
		{
			pe_params_articulated_body pab1;
			pab1.pivot.zero();
			pab1.pHost = pHost;
			pab1.posHostPivot = Vec3( GetPoseData().m_pJointsAbsolute[getBonePhysChildIndex(0)].t)*(scale/m_pInstance->m_RenderCharLocation.s)+offset;
			pab1.qHostPivot.SetIdentity();
			pab1.bAwake = 0;

			m_pCharPhysics->SetParams(&pab1);
		}

		pp.iSimClass = 4;
		m_pCharPhysics->SetParams(&pp);
	}

	m_vOffset = offset;
	m_fScale = scale;
	m_fMass = mass;
	m_iSurfaceIdx = surface_idx;
	m_stiffnessScale = stiffness_scale;
	m_pPrevCharHost = pHost;
	m_bPhysicsRelinquished = 0;
	m_timeStandingUp = -1.0f;

	/*IAttachmentObject* pAttachment;
	for(int i=m_AttachmentManager.GetAttachmentCount()-1; i>=0; i--)
	if ((pAttachment = m_AttachmentManager.GetInterfaceByIndex(i)->GetIAttachmentObject()) && pAttachment->GetICharacterInstance())
	pAttachment->GetICharacterInstance()->CreateCharacterPhysics(m_pCharPhysics, mass,surface_idx, stiffness_scale, nLod);*/

	return m_pCharPhysics;
}

int CSkeletonPose::CreateAuxilaryPhysics(IPhysicalEntity *pHost, const Matrix34 &mtx, int nLod)
{ 
	return CreateAuxilaryPhysics(pHost,mtx,mtx.GetColumn(0).len(),m_vOffset,nLod);
}


void ParsePhysInfoProps(CModelJoint *pBoneInfo,int nLod, pe_params_rope &pr, pe_simulation_params &simp, pe_params_flags &pf, const Quat &qrel)
{
	pr.jointLimit = pBoneInfo->m_PhysInfo[nLod].spring_tension[0];
	if (pBoneInfo->m_PhysInfo[nLod].min[0]!=0)
		pr.jointLimit = fabs_tpl(pBoneInfo->m_PhysInfo[nLod].min[0]);
	if (pBoneInfo->m_PhysInfo[nLod].min[1]!=0)
		pr.mass = fabs_tpl(RAD2DEG(pBoneInfo->m_PhysInfo[nLod].min[1]));
	if (pBoneInfo->m_PhysInfo[nLod].min[2]!=0)
		pr.collDist = fabs_tpl(RAD2DEG(pBoneInfo->m_PhysInfo[nLod].min[2]));
	simp.maxTimeStep = pBoneInfo->m_PhysInfo[nLod].spring_tension[1];
	if (simp.maxTimeStep<=0 || simp.maxTimeStep>=1)
		simp.maxTimeStep = 0.02f;
	pr.stiffnessAnim = max(0.001f, RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[0]));
	pr.stiffnessDecayAnim = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[1]);
	pr.dampingAnim = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[2]);
	pr.friction = pBoneInfo->m_PhysInfo[nLod].spring_tension[2];
	if (fabs_tpl(pBoneInfo->m_PhysInfo[nLod].spring_angle[2])<=10.0f)
		pr.jointLimitDecay = -pBoneInfo->m_PhysInfo[nLod].spring_angle[2];
	pr.bTargetPoseActive = 2*isneg(-pr.stiffnessAnim);
	pf.flagsOR = pef_traceable;
	if (!(pBoneInfo->m_PhysInfo[nLod].flags & 4))
		pr.bTargetPoseActive >>= 1;
	pf.flagsOR |= rope_target_vtx_rel0;
	pr.flagsCollider = geom_colltype_ray;
	if (!(pBoneInfo->m_PhysInfo[nLod].flags & 1))
		pf.flagsOR |= rope_collides;
	if (!(pBoneInfo->m_PhysInfo[nLod].flags & 2))
		pf.flagsOR |= rope_collides_with_attachment;
	if (pBoneInfo->m_PhysInfo[nLod].flags & joint_no_gravity)
	{
		pf.flagsOR |= pef_ignore_areas;
		simp.gravity.zero();
	}	else
		MARK_UNUSED simp.gravity;
	if (pBoneInfo->m_PhysInfo[nLod].flags & 8)
		pr.hingeAxis = qrel*Vec3(0,1,0);
	if (pBoneInfo->m_PhysInfo[nLod].flags & 16)
		pr.hingeAxis = qrel*Vec3(0,0,1);
}

void ParsePhysInfoPropsCloth(CModelJoint *pBoneInfo,int nLod, pe_params_softbody &psb, pe_simulation_params &simp)
{
	assert(nLod<sizeof(pBoneInfo->m_PhysInfo)/sizeof(pBoneInfo->m_PhysInfo[0]));
	psb.collTypes = nLod ? ent_terrain|ent_static|ent_rigid|ent_sleeping_rigid|ent_living : ent_independent;
	if (!(simp.maxTimeStep = pBoneInfo->m_PhysInfo[nLod].damping[0]))
		simp.maxTimeStep = 0.02f;
	if (!(psb.maxSafeStep = pBoneInfo->m_PhysInfo[nLod].damping[1]))
		psb.maxSafeStep = 0.1f;
	if (!(psb.ks = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[2])))
		psb.ks = 10.0f;
	if (!(psb.thickness = pBoneInfo->m_PhysInfo[nLod].damping[2]))
		psb.thickness = 0.04f;
	psb.friction = pBoneInfo->m_PhysInfo[nLod].spring_tension[2];
	psb.shapeStiffnessNorm = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[0]);
	psb.shapeStiffnessTang = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[1]);
	psb.airResistance = pBoneInfo->m_PhysInfo[nLod].spring_tension[1];
	psb.stiffnessAnim = pBoneInfo->m_PhysInfo[nLod].min[0];
	psb.massDecay = psb.stiffnessDecayAnim = pBoneInfo->m_PhysInfo[nLod].min[1];
	psb.dampingAnim = pBoneInfo->m_PhysInfo[nLod].min[2];
	psb.maxDistAnim = pBoneInfo->m_PhysInfo[nLod].spring_angle[2];
	simp.damping = pBoneInfo->m_PhysInfo[nLod].spring_tension[0];
	MARK_UNUSED simp.gravity;
	simp.minEnergy = nLod ? 0.1f:0.0f;
}


int CSkeletonPose::CreateAuxilaryPhysics(IPhysicalEntity *pHost, const Matrix34 &mtx, float scale,Vec3 offset, int nLod) 
//float *pForcedRopeLen,int *piForcedRopeIdx,int nForcedRopes)
{
	m_pSkeletonAnim->FinishAnimationComputations();

	int i,j,k,nchars;
	const char *pspace;

	// Delete aux physics

	if (m_nAuxPhys)
	{
		for(i=0;i<m_nAuxPhys;i++)
		{
			pe_params_rope pr;
			if (m_auxPhys[i].bTied0)
				pr.pEntTiedTo[0] = pHost;
			if (m_auxPhys[i].bTied1)
				pr.pEntTiedTo[1] = pHost;
			pr.bTargetPoseActive = 0;
			m_auxPhys[i].pPhysEnt->SetParams(&pr);
			//g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[i].pPhysEnt);
			//delete[] m_auxPhys[i].pauxBoneInfo;
			//delete[] m_auxPhys[i].pVtx;
		}
		return m_nAuxPhys;
	}

	m_nAuxPhys=0;

	for (i = 0; i<(int)GetJointCount(); i++)
	{
		CModelJoint* pBoneInfo = GetModelJointPointer(i);
		const char* szBoneName = pBoneInfo->GetJointName();

		if (!strnicmp(szBoneName,"rope",4) && pBoneInfo->numChildren()>0)
		{
			if ((pspace = strchr(szBoneName,' ')) || (pspace = strchr(szBoneName,'_')))
				nchars = (int)(pspace-szBoneName);
			else
				nchars = strlen(szBoneName);
			for(j = 0; j < m_nAuxPhys && strncmp(m_auxPhys[j].strName,szBoneName,nchars); ++j);
			if (j == m_nAuxPhys)
			{
				if (m_nAuxPhys==SIZEOF_ARRAY(m_auxPhys))
					break;
				m_auxPhys[m_nAuxPhys].pPhysEnt = g_pIPhysicalWorld->CreatePhysicalEntity(PE_ROPE);
				m_auxPhys[m_nAuxPhys].strName = szBoneName;
				m_auxPhys[m_nAuxPhys++].nChars = nchars;
			}
			pBoneInfo->m_nLimbId = 100+j;
		} else if (!pBoneInfo->getPhysInfo(0).pPhysGeom)
			pBoneInfo->getPhysInfo(0).flags = -1;
	}

	pe_status_pos sp;
	sp.pos = mtx.GetTranslation(); 
	sp.q = Quat(Matrix33(mtx)/mtx.GetColumn(0).GetLength());

	pe_params_flags pf;
	pf.flagsOR = pef_disabled;
	pe_action_target_vtx atv;
	pe_simulation_params simp;
	pe_params_softbody psb;
	pe_params_outer_entity poe;
	CModelJoint* pBoneInfo;
	simp.gravity.zero();
	poe.pOuterEntity = pHost;

	for (j = 0; j < m_nAuxPhys; ++j)
	{
		pe_params_rope pr;
		int iLastBone=0,iFirstBone=0;
		int bCloth=0,nStrands=0,iSavedRoot,i1,imin;
		pr.nSegments = 0;
		for(i=0;i<(int)GetJointCount();i++) 
			if (!strncmp(m_auxPhys[j].strName,GetModelJointPointer(i)->GetJointName(),m_auxPhys[j].nChars)) {
				if (GetModelJointPointer(i)->numChildren()>0)
					pr.nSegments++;
				if (i==0 || strncmp(m_auxPhys[j].strName,GetModelJointPointer(getBoneParentIndex(i))->GetJointName(),m_auxPhys[j].nChars))
					pr.nSegments++;
			}
		pr.pPoints = new Vec3[--pr.nSegments+1];
		m_auxPhys[j].pauxBoneInfo = new aux_bone_info[(m_auxPhys[j].nBones = pr.nSegments)+1];
		m_auxPhys[j].pVtx = pr.pPoints.data; 
		pr.length = 0;
		m_auxPhys[j].bTied0 = m_auxPhys[j].bTied1 = false;
		m_auxPhys[j].iBoneTiedTo[0] = m_auxPhys[j].iBoneTiedTo[1] = -1;
		m_auxPhys[j].pSubVtx=0; m_auxPhys[j].nSubVtxAlloc=0;
		m_auxPhys[j].iBoneStiffnessController = -1;

		for(i=k=0;i<(int)GetJointCount();) 
			if (!strncmp(m_auxPhys[j].strName,GetModelJointPointer(i)->GetJointName(),m_auxPhys[j].nChars) && GetModelJointPointer(i)->numChildren()>0) 
			{
				if (k==0 && getBonePhysParentIndex(iFirstBone=i,nLod)>=0)
					pr.idPartTiedTo[0] = getBonePhysParentIndex(m_auxPhys[j].iBoneTiedTo[0]=i,nLod);
				if (k>0 && !bCloth && i!=iLastBone)//strnicmp(GetModelJointPointer(getBoneParentIndex(i))->GetJointName(),"rope",4)) 
				{
					for(i1=0,imin=i;i1<(int)GetJointCount();i1++)
						if (!strncmp(m_auxPhys[j].strName,GetModelJointPointer(i1)->GetJointName(),m_auxPhys[j].nChars) && 
								strncmp(m_auxPhys[j].strName,GetModelJointPointer(getBoneParentIndex(i1))->GetJointName(),m_auxPhys[j].nChars) &&
								strcmp(GetModelJointPointer(i1)->GetJointName(),GetModelJointPointer(imin)->GetJointName())<0)
							imin=i1;
						if (imin!=i)
							iSavedRoot=0,i=imin,k=0;
						else
							iSavedRoot=i,i=GetModelJointChildIndex(m_auxPhys[j].pauxBoneInfo[k-1].iBone,0); 
					bCloth = 1;	continue;
				}

				pr.pPoints[k] = GetPoseData().m_pJointsAbsolute[i].t*scale + offset;
				pr.pPoints[k+1] = GetPoseData().m_pJointsAbsolute[GetModelJointChildIndex(i,0)].t * scale + offset;
				((GetModelJointPointer(i)->getPhysInfo(0).flags) &= 0xFFFF) |= 0x30000;

				m_auxPhys[j].pauxBoneInfo[k].iBone = i;
				m_auxPhys[j].pauxBoneInfo[k].dir0 = pr.pPoints[k+1]-pr.pPoints[k];
				if (m_auxPhys[j].pauxBoneInfo[k].dir0.len2()>0)
					m_auxPhys[j].pauxBoneInfo[k].dir0 *= 
					(m_auxPhys[j].pauxBoneInfo[k].rlen0 = 1.0f/m_auxPhys[j].pauxBoneInfo[k].dir0.GetLength());
				else {
					m_auxPhys[j].pauxBoneInfo[k].dir0(0,0,1);
					m_auxPhys[j].pauxBoneInfo[k].rlen0 = 1.0f;
				}
				/*if (!is_unused(pr.idPartTiedTo[0]))
				{
					m_auxPhys[j].pauxBoneInfo[k].dir0 = !GetPoseData().m_jointsAbsolute[pr.idPartTiedTo[0]].q*m_auxPhys[j].pauxBoneInfo[k].dir0;
					m_auxPhys[j].pauxBoneInfo[k].quat0 = !GetPoseData().m_jointsAbsolute[pr.idPartTiedTo[0]].q*GetPoseData().m_jointsAbsolute[i].q;
				} else
					m_auxPhys[j].pauxBoneInfo[k].quat0 = GetPoseData().m_jointsAbsolute[i].q;*/
				m_auxPhys[j].pauxBoneInfo[k].quat0 = GetPoseData().m_pJointsAbsolute[i].q;
				//m_auxPhys[j].pauxBoneInfo[k].dir0 = !GetPoseData().m_pJointsAbsolute[getBoneParentIndex(i)].q*m_auxPhys[j].pauxBoneInfo[k].dir0;
				//m_auxPhys[j].pauxBoneInfo[k].quat0 = !GetPoseData().m_pJointsAbsolute[getBoneParentIndex(i)].q*GetPoseData().m_pJointsAbsolute[i].q;

				//Matrix34 q34 = Matrix34(m_arrJoints[i].GetPoseData().m_jointsAbsolute);
				//m_auxPhys[j].pauxBoneInfo[k].quat0 = Quat(q34);

				pr.pPoints[k] = sp.q*pr.pPoints[k]+sp.pos;
				pr.pPoints[k+1] = sp.q*pr.pPoints[k+1]+sp.pos;
				pr.length += (pr.pPoints[k+1]-pr.pPoints[k]).GetLength();
				iLastBone = GetModelJointChildIndex(i,0);
				m_auxPhys[j].pauxBoneInfo[++k].iBone = iLastBone;
				m_auxPhys[j].pauxBoneInfo[k].dir0(0,0,1); m_auxPhys[j].pauxBoneInfo[k].rlen0=1.0f; m_auxPhys[j].pauxBoneInfo[k].quat0.SetIdentity();
				if (bCloth) {
					if (strncmp(m_auxPhys[j].strName,GetModelJointPointer(i=iLastBone)->GetJointName(),m_auxPhys[j].nChars) || 
							GetModelJointPointer(i)->numChildren()==0)
					{
						for(i=iSavedRoot,imin=-1; i<(int)GetJointCount(); i++)
							if (!strncmp(m_auxPhys[j].strName,GetModelJointPointer(i)->GetJointName(),m_auxPhys[j].nChars) && 
									strncmp(m_auxPhys[j].strName,GetModelJointPointer(getBoneParentIndex(i))->GetJointName(),m_auxPhys[j].nChars))
							{
								for(i1=0; i1<k && m_auxPhys[j].pauxBoneInfo[i1].iBone!=i; i1++);
								if (i1==k && (imin<0 || strcmp(GetModelJointPointer(i)->GetJointName(),GetModelJointPointer(imin)->GetJointName())<0))
									imin = i;
							}
						if (imin>=0)
							i = imin;
						++nStrands;	++k;
					}
				} else
					i = iLastBone;
			}	else
				++i;
		if (!bCloth && k!=pr.nSegments)
		{
			g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[j].pPhysEnt);

			delete[] m_auxPhys[j].pauxBoneInfo;
			m_auxPhys[j].pauxBoneInfo = NULL;

			delete[] m_auxPhys[j].pVtx;
			m_auxPhys[j].pVtx = NULL;

			memmove(m_auxPhys+j, m_auxPhys+j+1, (m_nAuxPhys-1-j)*sizeof(m_auxPhys[0]));
			--m_nAuxPhys; --j;
			continue;
		}

		if (!is_unused(pr.idPartTiedTo[0])) 
		{
			pr.pEntTiedTo[0] = pHost;
			pr.ptTiedTo[0] = pr.pPoints[0];
			m_auxPhys[j].bTied0 = true;
		}

		if (bCloth) 
		{
			uint16 *pIdx = new uint16[k*6];
			int nTris=0,i2,i3,i0,idx;
			for(i1=1; i1<k && !strncmp(GetModelJointPointer(getBoneParentIndex(m_auxPhys[j].pauxBoneInfo[i1].iBone))->GetJointName(),
																 m_auxPhys[j].strName,m_auxPhys[j].nChars); i1++);
			for(i=0,i0=i1; i<k; i=i1,i1=i3) {
				if (i1==k) {
					if (!strstr(m_auxPhys[j].strName,"loop"))
						break;
					else
						i2=0,i3=i0;
				} else
					for(i3=(i2=i1)+1; i3<k && !strncmp(GetModelJointPointer(getBoneParentIndex(m_auxPhys[j].pauxBoneInfo[i3].iBone))->GetJointName(),
																						 m_auxPhys[j].strName,m_auxPhys[j].nChars); i3++);
				for(idx=0; idx<i1-i-1 && idx<i3-i2-1; idx++) {
					pIdx[nTris*3]=i+idx;	 pIdx[nTris*3+1]=i+idx+1;	 pIdx[nTris*3+2]=i2+idx; nTris++;
					pIdx[nTris*3]=i+idx+1; pIdx[nTris*3+1]=i2+idx+1; pIdx[nTris*3+2]=i2+idx; nTris++;
				}
				if (idx<i1-i-1) for(;idx<i1-i-1;idx++) {
					pIdx[nTris*3]=i+idx; pIdx[nTris*3+1]=i+idx+1; pIdx[nTris*3+2]=i3-1;		nTris++;
				}	else if (idx<i3-i2-1) for(;idx<i3-i2-1;idx++) {
					pIdx[nTris*3]=i1-1; pIdx[nTris*3+1]=i2+idx+1; pIdx[nTris*3+2]=i2+idx; nTris++;
				}
			}
			for(i=0;i<k;i++)
				pr.pPoints[i] -= sp.pos;
			IGeometry *pMesh = g_pIPhysicalWorld->GetGeomManager()->CreateMesh(pr.pPoints, pIdx, 0,0, nTris, mesh_SingleBB);
			// get defSurfaceIdx from udp mat?
			phys_geometry *pgeom = g_pIPhysicalWorld->GetGeomManager()->RegisterGeometry(pMesh);
			pgeom->nRefCount = 0;
			g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[j].pPhysEnt);
			pe_params_pos pp; pp.pos=sp.pos;
			m_auxPhys[j].pPhysEnt = g_pIPhysicalWorld->CreatePhysicalEntity(PE_SOFT,&pp);
			pe_geomparams gp;	gp.mass=1.0f; gp.density=800.0f;
			gp.flagsCollider = geom_colltype6;
			m_auxPhys[j].pPhysEnt->AddGeometry(pgeom, &gp);

#ifndef CONSOLE_CONST_CVAR_MODE
			if (Console::GetInst().ca_cloth_vars_reset==0) 
			{
				pBoneInfo->m_PhysInfo[nLod].damping[0] = Console::GetInst().ca_cloth_max_timestep;
				pBoneInfo->m_PhysInfo[nLod].damping[1] = Console::GetInst().ca_cloth_max_safe_step;
				pBoneInfo->m_PhysInfo[nLod].max[2] = DEG2RAD(Console::GetInst().ca_cloth_stiffness);
				pBoneInfo->m_PhysInfo[nLod].damping[2] = Console::GetInst().ca_cloth_thickness;
				pBoneInfo->m_PhysInfo[nLod].spring_tension[2]	= Console::GetInst().ca_cloth_friction;
				pBoneInfo->m_PhysInfo[nLod].max[0] = DEG2RAD(Console::GetInst().ca_cloth_stiffness_norm);
				pBoneInfo->m_PhysInfo[nLod].max[1] = DEG2RAD(Console::GetInst().ca_cloth_stiffness_tang);
				pBoneInfo->m_PhysInfo[nLod].spring_tension[0] = Console::GetInst().ca_cloth_damping;
				pBoneInfo->m_PhysInfo[nLod].spring_tension[1] = Console::GetInst().ca_cloth_air_resistance;
			} else if (Console::GetInst().ca_cloth_vars_reset==1) 
			{
				Console::GetInst().ca_cloth_max_timestep = pBoneInfo->m_PhysInfo[nLod].damping[0];
				Console::GetInst().ca_cloth_max_safe_step = pBoneInfo->m_PhysInfo[nLod].damping[1];
				Console::GetInst().ca_cloth_stiffness = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[2]);
				Console::GetInst().ca_cloth_thickness = pBoneInfo->m_PhysInfo[nLod].damping[2];
				Console::GetInst().ca_cloth_friction = pBoneInfo->m_PhysInfo[nLod].spring_tension[2];
				Console::GetInst().ca_cloth_stiffness_norm = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[0]);
				Console::GetInst().ca_cloth_stiffness_tang = RAD2DEG(pBoneInfo->m_PhysInfo[nLod].max[1]);
				Console::GetInst().ca_cloth_damping = pBoneInfo->m_PhysInfo[nLod].spring_tension[0];
				Console::GetInst().ca_cloth_air_resistance = pBoneInfo->m_PhysInfo[nLod].spring_tension[1];
				Console::GetInst().ca_cloth_vars_reset = 0;
			}
#endif

			pBoneInfo = GetModelJointPointer(iFirstBone);
			ParsePhysInfoPropsCloth(pBoneInfo,nLod,psb,simp);
			m_auxPhys[j].pPhysEnt->SetParams(&psb);
			m_auxPhys[j].pPhysEnt->SetParams(&simp);
			m_auxPhys[j].pPhysEnt->SetParams(&poe);
			MARK_UNUSED simp.damping,simp.maxTimeStep,simp.minEnergy;
			simp.gravity.zero();

			pe_action_attach_points aap;
			aap.pEntity = pHost;
			aap.nPoints = 1;
			aap.piVtx = &i;
			for(i=0;i<k;) {
				aap.partid = getBonePhysParentIndex(m_auxPhys[j].pauxBoneInfo[i].iBone,nLod);
				m_auxPhys[j].pPhysEnt->Action(&aap);
				for(i++; i<k && !strncmp(GetModelJointPointer(getBoneParentIndex(m_auxPhys[j].pauxBoneInfo[i].iBone))->GetJointName(),
																 m_auxPhys[j].strName,m_auxPhys[j].nChars); i++);
			}

			if (psb.stiffnessAnim>0.0f)
			{
				QuatT qBase = GetPoseData().m_pJointsAbsolute[pr.idPartTiedTo[is_unused(pr.idPartTiedTo[0])]].GetInverted();
				atv.points = m_auxPhys[j].pVtx;
				atv.nPoints = m_auxPhys[j].nBones+1;
				for(i=0;i<m_auxPhys[j].nBones;i++) 
				{
					k = m_auxPhys[j].pauxBoneInfo[i].iBone;
					atv.points[i] = qBase*GetPoseData().m_pJointsAbsolute[k].t;
					atv.points[i+1] = qBase*GetPoseData().m_pJointsAbsolute[GetModelJointChildIndex(k,0)].t;
				}
				m_auxPhys[j].pPhysEnt->Action(&atv);
			}

			continue;
		}

		char boneAttachName[128];
		int iOtherBone = -1;
		sprintf(boneAttachName,"attach_%.*s", m_auxPhys[j].nChars,m_auxPhys[j].strName);
		if ((iOtherBone=GetJointIDByName(boneAttachName))>=0 && (i=getBonePhysParentOrSelfIndex(iOtherBone,nLod))>=0 || (i=getBonePhysChildIndex(iLastBone))>=0) 
		{
			pr.pEntTiedTo[1] = pHost;
			m_auxPhys[j].iBoneTiedTo[1] = iOtherBone>=0 ? -iOtherBone-1 : iLastBone;
			pr.idPartTiedTo[1] = i;
			pr.ptTiedTo[1] = pr.pPoints[pr.nSegments];
			m_auxPhys[j].bTied1 = true;
		}
		//if (pForcedRopeLen && *piForcedRopeIdx+i<nForcedRopes)
		//	pr.length = pForcedRopeLen[*piForcedRopeIdx+j];

		pBoneInfo = GetModelJointPointer(iFirstBone);
		if (!(pBoneInfo->m_PhysInfo[nLod].flags & joint_isolated_accelerations))
		{
			ParsePhysInfoProps(pBoneInfo,nLod, pr,simp,pf, m_pModelSkeleton->GetPoseData().GetJointsRelative()[iFirstBone].q);
			if (pBoneInfo->m_PhysInfo[nLod].framemtx[0][1]>=2.0f && pBoneInfo->m_PhysInfo[nLod].framemtx[0][1]<100.0f)
			{
				sprintf(boneAttachName, "rope_stiffness_controller_%d", FtoI(pBoneInfo->m_PhysInfo[nLod].framemtx[0][1]-1.0f));
				m_auxPhys[j].iBoneStiffnessController = GetJointIDByName(boneAttachName);
			}
			m_auxPhys[j].bPhysical = true;
		}	else
		{
			MARK_UNUSED pr.bTargetPoseActive,pr.jointLimit,pr.stiffnessAnim,pr.dampingAnim,pr.stiffnessDecayAnim,simp.gravity,simp.maxTimeStep;
			m_auxPhys[j].bPhysical = false;
		}
		pr.surface_idx = *(int*)(pBoneInfo->m_PhysInfo[nLod].spring_angle+1);

		m_auxPhys[j].pPhysEnt->SetParams(&pr);
		m_auxPhys[j].pPhysEnt->SetParams(&simp);
		m_auxPhys[j].pPhysEnt->SetParams(&poe);
		if (m_nAuxPhys>1 || m_auxPhys[j].bPhysical)
		{
			m_auxPhys[j].pPhysEnt->SetParams(&pf);
			pe_action_awake aa; aa.bAwake=m_auxPhys[j].bPhysical;
			m_auxPhys[j].pPhysEnt->Action(&aa);
		}
		if (pf.flagsOR & rope_target_vtx_rel0)
		{
			QuatT qBase = GetPoseData().m_pJointsAbsolute[pr.idPartTiedTo[is_unused(pr.idPartTiedTo[0])]].GetInverted();
			atv.points = m_auxPhys[j].pVtx;
			atv.nPoints = m_auxPhys[j].nBones+1;
			for(i=0;i<m_auxPhys[j].nBones;i++) 
			{
				k = m_auxPhys[j].pauxBoneInfo[i].iBone;
				atv.points[i] = qBase*GetPoseData().m_pJointsAbsolute[k].t;
				atv.points[i+1] = qBase*GetPoseData().m_pJointsAbsolute[GetModelJointChildIndex(k,0)].t;
			}
			m_auxPhys[j].pPhysEnt->Action(&atv);
		}
	}

	/*if (nLod==1) 
	{
		int nAuxPhys;
		for(i=nAuxPhys=0;i<m_nAuxPhys;i++)
		{
			for(j=0;j<m_auxPhys[i].nBones && !GetModelJointPointer(m_auxPhys[i].pauxBoneInfo[j].iBone)->m_PhysInfo[nLod].pPhysGeom;j++);
			if (j<m_auxPhys[i].nBones)
			{
				g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[i].pPhysEnt);
				delete[] m_auxPhys[i].pauxBoneInfo;
				delete[] m_auxPhys[i].pVtx;
			} else
				m_auxPhys[nAuxPhys++] = m_auxPhys[i];
		}
		m_nAuxPhys = nAuxPhys;
	}*/

	m_vOffset = offset;
	m_fScale = scale;

	//if (pForcedRopeLen)
	//	*piForcedRopeIdx += j;
	IAttachmentObject* pAttachment;
	ICharacterInstance *pChar;
	IPhysicalEntity *pRope;
	for(int ic=m_pInstance->m_AttachmentManager.GetAttachmentCount()-1; ic>=0; ic--)
	{
		if (m_pInstance->m_AttachmentManager.GetInterfaceByIndex(ic)->GetType()==CA_BONE && (pAttachment = m_pInstance->m_AttachmentManager.GetInterfaceByIndex(ic)->GetIAttachmentObject()) && (pChar=pAttachment->GetICharacterInstance()))
		{
			((CCharInstance*)pChar)->m_SkeletonPose.CreateAuxilaryPhysics(pHost, mtx, scale,offset, nLod);//, pForcedRopeLen,piForcedRopeIdx,nForcedRopes);
			for(int j0=0; pRope=pChar->GetISkeletonPose()->GetCharacterPhysics(j0); j0++)
			{
				pe_params_rope pr;
				pr.pEntTiedTo[0] = pHost;
				pr.idPartTiedTo[0] = getBonePhysParentOrSelfIndex(m_pInstance->m_AttachmentManager.GetInterfaceByIndex(ic)->GetBoneID());
				pRope->SetParams(&pr);
				new (&atv) pe_action_target_vtx;
				pRope->Action(&atv);
			}
		}
	}
		return m_nAuxPhys;
}


int CSkeletonPose::FillRopeLenArray(float *len,int i0,int sz)
{
	int i,i0org=i0;
	pe_params_rope pr;
	for(i=0;i<m_nAuxPhys && i+i0<sz;i++)
	{
		int32 status = m_auxPhys[i].pPhysEnt->GetParams(&pr);
		if (status==0)
		{
			g_pISystem->Warning( VALIDATOR_MODULE_ANIMATION,VALIDATOR_WARNING, VALIDATOR_FLAG_FILE, m_pInstance->m_pModel->GetModelFilePath(), "GetParams() returned 0" );
			assert(0);
		}
		len[i0+i] = pr.length;
	}
	i0+=i;

	IAttachmentObject* pAttachment;
	ICharacterInstance *pChar;
	for(i=m_pInstance->m_AttachmentManager.GetAttachmentCount()-1; i>=0; i--)
		if (m_pInstance->m_AttachmentManager.GetInterfaceByIndex(i)->GetType()==CA_BONE && 
			(pAttachment = m_pInstance->m_AttachmentManager.GetInterfaceByIndex(i)->GetIAttachmentObject()) && (pChar=pAttachment->GetICharacterInstance()))
			i0 += ((CCharInstance*)pChar)->m_SkeletonPose.FillRopeLenArray(len,i0,sz);
	return i0-i0org;
}

void CSkeletonPose::InitializeAnimToPhysIndexArray()
{
	if (m_nPhysJoints > -1)
		return;

	uint32 jointCount = GetJointCount();
	m_nPhysJoints = 0;

	for (uint32 i=0; i<jointCount; ++i)
	{
		if (GetModelJointPointer(i)->m_PhysInfo[0].pPhysGeom)
			m_nPhysJoints++;
	}

	if (!m_nPhysJoints)
		return;
	
	m_physJoints.resize(m_nPhysJoints);
	m_physJointsIdx.resize(m_nPhysJoints);
	uint32 j = 0;
	for(uint32 i=0; i<jointCount; ++i)
	{
		if (GetModelJointPointer(i)->m_PhysInfo[0].pPhysGeom)
			m_physJointsIdx[j++] = i;
	}
}

void CSkeletonPose::SynchronizeWithPhysicalEntity(Skeleton::CPoseData& poseData, IPhysicalEntity *pent, const Vec3& posMaster,const Quat& qMaster, QuatT offset, int iDir )
{
	if (iDir==0 || (iDir<0 && pent->GetType()!=PE_ARTICULATED && pent->GetType()!=PE_RIGID))
	{
		if (pent)
		{
			Physics_SynchronizeToEntity(*pent, offset);
		}
	}
	else
	{
		SPU_Physics_SynchronizeFromEntity(poseData, pent, posMaster, qMaster, offset);
	}
}

// IZF: We use this for calls outside CryAnimation, which is bad since this
// might write into the pose data as we are processing it in an animation task.
void CSkeletonPose::SynchronizeWithPhysicalEntity(IPhysicalEntity *pent, const Vec3& posMaster,const Quat& qMaster)
{
	Skeleton::CPoseData& poseDataWriteable = GetPoseDataForceWriteable();

	SynchronizeWithPhysicsPrepare(*CSkeletonAnimTask::GetMemoryPool(), QuatT(IDENTITY), QuatTS(IDENTITY));
	SynchronizeWithPhysicalEntity(poseDataWriteable, pent, posMaster, qMaster, QuatT(IDENTITY));
	UpdateBBox(1);
}

IPhysicalEntity *CSkeletonPose::RelinquishCharacterPhysics(const Matrix34 &mtx, float stiffness, bool bCopyJointVelocities, const Vec3 &velHost) 
{
	Skeleton::CPoseData& poseDataWriteable = GetPoseDataForceWriteable();

#ifndef _RELEASE	
	poseDataWriteable.ValidateRelPose(m_parrModelJoints);
#endif

	if (!m_bHasPhysics) 
		return 0;

	int nLod = GetPhysicsLod();
	int i,j,nAuxPhys, iRoot=getBonePhysChildIndex(0,0);
	//float ropeLen[64];
	//FillRopeLenArray(ropeLen,0,64);
	//DestroyCharacterPhysics();
	m_bPhysicsRelinquished = 1;

	if (m_arrPhysicsJoints.empty())
		InitPhysicsSkeleton();
	poseDataWriteable.ComputeRelativePose(m_parrModelJoints);

	if (m_bSetRagdollDefaultPoseRequested)
		DoSetRagdollDefaultPose(poseDataWriteable);

	// store death pose (current) matRelative orientation in bone's m_pqTransform
	for(i=0;i<(int)GetJointCount();i++) 
		m_arrPhysicsJoints[i].m_qRelFallPlay = poseDataWriteable.m_pJointsRelative[i].q;

	ResetNonphysicalBoneRotations(poseDataWriteable, nLod, 1.0f); // reset nonphysical bones matRel to default pose matRel
	UnconvertBoneGlobalFromRelativeForm(poseDataWriteable, false, nLod); // build matGlobals from matRelativeToParents 

	pe_params_articulated_body pab;
	pab.bGrounded = 0;
	pab.scaleBounceResponse = 1;
	pab.bCheckCollisions = 1;
	pab.bCollisionResp = 1;

	IPhysicalEntity *res = g_pIPhysicalWorld->CreatePhysicalEntity(PE_ARTICULATED,&pab);//,&pp,0);
	pe_simulation_params sp; 
	sp.iSimClass = 6;	// to make sure the entity is not processed until it's ready (multithreaded)
	res->SetParams(&sp);
	m_vOffset.zero();
	BuildPhysicalEntity(res, m_fMass, m_iSurfaceIdx,stiffness, nLod,0, Matrix34(Vec3(m_fScale),Quat(IDENTITY), Vec3(ZERO) ));

	pe_params_joint pj;
	pj.bNoUpdate = 1;
	iRoot = getBonePhysChildIndex(0,nLod);
	for(i=0;i<3;i++) { pj.kd[i]=0.1f; }//pj.ks[i]=0; }
	for(i=GetJointCount()-1;i>=0;i--)	
		if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom) 
		{
			pj.op[0] = getBonePhysParentIndex(i,nLod);
			pj.op[1] = i;
			pj.flags = GetModelJointPointer(i)->m_PhysInfo[nLod].flags & (all_angles_locked | angle0_limit_reached*7);
			if (i==iRoot)
				pj.flags = 0;
			res->SetParams(&pj);
		}

	m_fPhysBlendMaxTime = Console::GetInst().ca_DeathBlendTime;
	if (m_fPhysBlendMaxTime>0.001f)
		m_frPhysBlendMaxTime = 1.0f/m_fPhysBlendMaxTime;
	else
	{
		m_fPhysBlendMaxTime = 0.0f;
		m_frPhysBlendMaxTime = 1.0f;
	}

	// No blending if this ragdoll is caused by loading a checkpoint
	m_fPhysBlendTime = !gEnv->pSystem->IsSerializingFile() ? 0.0f : m_fPhysBlendMaxTime;

	ResetNonphysicalBoneRotations(poseDataWriteable, nLod, 0.0f); // restore death pose matRel from m_pqTransform
	UnconvertBoneGlobalFromRelativeForm(poseDataWriteable, false, nLod); // build matGlobals from matRelativeToParents
	
	m_vOffset.zero();
	m_bPhysicsAwake=m_bPhysicsWasAwake = 1; 
	//m_uFlags |= nFlagsNeedReskinAllLODs; // in order to force skinning update during the first death sequence frame
	pe_params_pos pp;
    pp.pos = mtx.GetTranslation();
	pp.q = Quat(Matrix33(mtx)/mtx.GetColumn(0).len());
	//pp.pMtx3x4 = (Matrix34*)&mtx;
	int bSkelQueued = res->SetParams(&pp)-1;
	SetForceSkeletonUpdate(33);

	/*sp.gravity.Set(0,0,-7.5f);
	sp.maxTimeStep = 0.025f;
	sp.damping = 0.1f;
	res->SetParams(&sp);*/

	//CreateAuxilaryPhysics(res, mtx, 1.0f,m_vOffset, 1, ropeLen,&(i=0),sizeof(ropeLen)/sizeof(ropeLen[0]));
	pe_params_rope pr;
	pe_params_flags pf;
	pe_params_timeout pto;
	pe_simulation_params spr;
	pe_params_outer_entity poe;
	pr.bLocalPtTied = 1;
	pto.maxTimeIdle = 4.0f;
	poe.pOuterEntity = res;
	for(i=nAuxPhys=0;i<m_nAuxPhys;i++)
	{
		for(j=0;j<m_auxPhys[i].nBones && !GetModelJointPointer(m_auxPhys[i].pauxBoneInfo[j].iBone)->m_PhysInfo[nLod].pPhysGeom;j++);
		if (j<m_auxPhys[i].nBones)
		{
			g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[i].pPhysEnt);

			delete[] m_auxPhys[i].pauxBoneInfo;
			m_auxPhys[i].pauxBoneInfo = NULL;

			delete[] m_auxPhys[i].pVtx;
			m_auxPhys[i].pVtx = NULL;
		}
		else
		{
			CModelJoint *pBoneInfo = GetModelJointPointer(m_auxPhys[i].pauxBoneInfo[0].iBone);
			if (m_auxPhys[i].pPhysEnt->GetType()==PE_SOFT)
			{
				pe_params_softbody psb;
				ParsePhysInfoPropsCloth(pBoneInfo,nLod,psb,spr);
				m_auxPhys[i].pPhysEnt->SetParams(&psb,-bSkelQueued>>31);

				pe_action_attach_points aap;
				aap.pEntity = 0;
				aap.piVtx = 0;
				m_auxPhys[i].pPhysEnt->Action(&aap,-bSkelQueued>>31);
				Vec3 pt; aap.points = &pt;
				aap.pEntity = res;
				aap.bLocal = 1;
				aap.nPoints = 1;
				aap.piVtx = &j;
				for(j=0; j<m_auxPhys[i].nBones; ) {
					aap.partid = getBonePhysParentIndex(m_auxPhys[i].pauxBoneInfo[j].iBone,nLod);
					pt = m_arrPhysicsJoints[m_auxPhys[i].pauxBoneInfo[j].iBone].m_DefaultRelativeQuat.t*m_fScale;
					m_auxPhys[i].pPhysEnt->Action(&aap,-bSkelQueued>>31);
					for(j++; j<m_auxPhys[i].nBones && !strncmp(GetModelJointPointer(getBoneParentIndex(m_auxPhys[i].pauxBoneInfo[j].iBone))->GetJointName(),
							m_auxPhys[i].strName,m_auxPhys[i].nChars); j++);
				}
			} else
			{
				if (!(pBoneInfo->m_PhysInfo[nLod].flags & joint_isolated_accelerations))
					ParsePhysInfoProps(pBoneInfo,nLod, pr,spr,pf, m_pModelSkeleton->GetPoseData().GetJointsRelative()[m_auxPhys[i].pauxBoneInfo[0].iBone].q);
				else
				{
					MARK_UNUSED pr.bTargetPoseActive,pr.jointLimit,pr.stiffnessAnim,pr.dampingAnim,pr.stiffnessDecayAnim,spr.gravity,spr.maxTimeStep;
					pr.bTargetPoseActive = 0;
					pr.collDist = 0.005f;
					pr.dampingAnim = 0.0f;
					spr.gravity = Vec3(0,0,-6.0f);
					spr.damping = 1.0f;
				}
				if (m_auxPhys[i].bTied0)
				{
					pr.pEntTiedTo[0] = res;
					pr.idPartTiedTo[0] = getBonePhysParentIndex(j = m_auxPhys[i].iBoneTiedTo[0],nLod);
					Vec3 ptTiedTo = poseDataWriteable.m_pJointsAbsolute[getBoneParentIndex(j)]*m_arrPhysicsJoints[j].m_DefaultRelativeQuat.t*m_fScale;
					ptTiedTo = poseDataWriteable.m_pJointsAbsolute[pr.idPartTiedTo[0]].GetInverted()*ptTiedTo;
					if (ptTiedTo.len2()<1.0f)
						pr.ptTiedTo[0] = ptTiedTo;
				}
				if (m_auxPhys[i].bTied1)
				{
					pr.pEntTiedTo[1] = res; 
					pr.idPartTiedTo[1] = m_auxPhys[i].iBoneTiedTo[1]>=0 ? getBonePhysChildIndex(m_auxPhys[i].iBoneTiedTo[1],nLod) :
						getBonePhysParentOrSelfIndex(-m_auxPhys[i].iBoneTiedTo[1]-1,nLod);
					pr.ptTiedTo[1] = poseDataWriteable.m_pJointsAbsolute[pr.idPartTiedTo[1]].t;
				}
				
				if (m_auxPhys[i].bPhysical)
					pf.flagsAND &= ~rope_ignore_attachments;
				m_auxPhys[i].pPhysEnt->SetParams(&pr,-bSkelQueued>>31);
			}
			pf.flagsAND = ~pef_ignore_areas;
			m_auxPhys[i].pPhysEnt->SetParams(&pf,-bSkelQueued>>31);
			m_auxPhys[i].pPhysEnt->SetParams(&spr,-bSkelQueued>>31);
			//if (is_unused(pr.stiffnessAnim) || pr.stiffnessAnim<=0.0f)
				m_auxPhys[i].pPhysEnt->SetParams(&pto,-bSkelQueued>>31);
			m_auxPhys[i].pPhysEnt->SetParams(&poe,-bSkelQueued>>31);
			m_auxPhys[nAuxPhys++] = m_auxPhys[i];
		}
	}
	m_nAuxPhys = nAuxPhys;

	MARK_UNUSED pr.pEntTiedTo[1],pr.idPartTiedTo[1];
	IAttachmentObject* pAttachment;
	ICharacterInstance *pChar;
	IPhysicalEntity *pRope;
	for(i=m_pInstance->m_AttachmentManager.GetAttachmentCount()-1; i>=0; i--)
		if (m_pInstance->m_AttachmentManager.GetInterfaceByIndex(i)->GetType()==CA_BONE && 
			(pAttachment = m_pInstance->m_AttachmentManager.GetInterfaceByIndex(i)->GetIAttachmentObject()) && (pChar=pAttachment->GetICharacterInstance()))
			for(j=0; pRope=pChar->GetISkeletonPose()->GetCharacterPhysics(j); j++)
			{
				pr.pEntTiedTo[0] = res;
				pr.idPartTiedTo[0] = getBonePhysParentOrSelfIndex(m_pInstance->m_AttachmentManager.GetInterfaceByIndex(i)->GetBoneID());
				pRope->SetParams(&pr);
				pRope->SetParams(&poe);
			}

	if (m_pCharPhysics)
	{	
		if (bCopyJointVelocities) 
		{ 
			// Copy part dynamics
			switch(Console::GetInst().ca_ApplyJointVelocitiesMode)
			{
			case Console::ApplyJointVelocitiesAnimation: 
				{
					m_pInstance->ApplyJointVelocitiesToPhysics(res,pp.q,velHost);
				} break;
			case Console::ApplyJointVelocitiesPhysics:
				{
					pe_status_dynamics partStatusSource;
					pe_action_set_velocity asv;
					for(i=GetJointCount()-1;i>=0;i--)	
						if (GetModelJointPointer(i)->m_PhysInfo[nLod].pPhysGeom) 
						{
							partStatusSource.partid = i;
							if (!m_pCharPhysics->GetStatus(&partStatusSource)) continue;
							asv.partid = i;
							asv.v = partStatusSource.v+velHost;
							asv.w = partStatusSource.w;
							res->Action(&asv);
						}
				} break;
			default: break;
			}
		}

		g_pIPhysicalWorld->DestroyPhysicalEntity(m_pCharPhysics);
	}
	if (m_ppBonePhysics)
	{
		for(i=0;i<(int)GetJointCount();i++) if (m_ppBonePhysics[i])
			m_ppBonePhysics[i]->Release(), g_pIPhysicalWorld->DestroyPhysicalEntity(m_ppBonePhysics[i]);
		delete[] m_ppBonePhysics;
		m_ppBonePhysics = 0;
	}

	m_pCharPhysics = res;
	m_bPhysicsRelinquished = 1;
	m_nSpineBones = 0;
	m_iFlyDir = -1;
	m_bAliveRagdoll = stiffness>0;
	m_bLimpRagdoll = 0;
	m_timeRagdolled = 0;
	m_timeLying=m_timeStandingUp = -1; m_timeNoColl=0;
	m_b3DOFStandup = 0;

	sp.iSimClass = 2;
	res->SetParams(&sp);

	return res;
}

bool CSkeletonPose::AddImpact(const int partid,Vec3 point,Vec3 impact)
{
	if (m_pCharPhysics)
	{
		if (!Console::GetInst().ca_physicsProcessImpact ||
			m_pInstance->m_pModel->m_ObjectType==CGA || (unsigned int)partid>=(unsigned int)GetJointCount())
		{
			// Exit early if physic impacts aren't processed
			// sometimes out of range value (like -1) is passed to indicate that no impulse should be added to character
			return false;
		}
		//int i;

		//SJoint* pImpactBone = &m_arrJoints[partid];
		const char* szImpactBoneName = GetModelJointPointer(partid)->GetJointName();

		//for(i=0; i<4 && !(m_pIKEffectors[i] && m_pIKEffectors[i]->AffectsBone(partid)); i++);
		const char *ptr = strstr(szImpactBoneName,"Spine");
		if (strstr(szImpactBoneName,"Pelvis") || ptr && !ptr[5])
			return false;

		if (strstr(szImpactBoneName,"UpperArm") || strstr(szImpactBoneName,"Forearm"))
			impact *= 0.35f;
		else if (strstr(szImpactBoneName,"Hand"))
			impact *= 0.2f;
		else if (strstr(szImpactBoneName,"Spine1"))
			impact *= 0.2f;

		pe_action_impulse impulse;
		impulse.partid = partid;
		impulse.impulse = impact;
		impulse.point = point;
		m_pCharPhysics->Action(&impulse);

		return true;
	}
	else 
	{
		pe_action_impulse impulse;
		impulse.impulse = impact;
		impulse.point = point;
		for(int i=0;i<m_nAuxPhys;i++)
			m_auxPhys[i].pPhysEnt->Action(&impulse);
	}

	return false;
}

// Forces skinning on the next frame
void CSkeletonPose::ForceReskin()
{
	//m_uFlags |= nFlagsNeedReskinAllLODs;
	m_bPhysicsWasAwake = true;
}

// Process

void CSkeletonPose::Physics_SynchronizeToAux(const Skeleton::CPoseData& poseData, const QuatT& locationPhysics, int nNeff, const QuatT& attachmentParent)
{
	if (!m_bPhysicsSynchronizeAux)
		return;
	if (!m_pPhysBuffer)
		return;

	pe_params_articulated_body pab;
	if (m_pCharPhysics)
		m_pCharPhysics->GetParams(&pab);

	int nLod = m_bPhysicsRelinquished ? GetPhysicsLod() : 0;

	pe_action_target_vtx atv;
	Matrix34 mtx = Matrix34(locationPhysics);
	int bRopesAwake = 0;
	for (int j=0; j<m_nAuxPhys; ++j)
	{
		const pe_type peType = m_auxPhys[j].pPhysEnt->GetType();

		QuatT qtRoot = attachmentParent;
		if (m_auxPhys[j].bTied0 && m_pCharPhysics)
		{
			int parentIndex = getBonePhysParentIndex(m_auxPhys[j].iBoneTiedTo[0], nLod);
			qtRoot = poseData.m_pJointsAbsolute[parentIndex];

			if (m_pPhysBuffer && m_bPhysBufferFilled)
				qtRoot = m_pPhysBuffer[parentIndex].location;
		}

		pe_params_rope pr;
		pr.bTargetPoseActive = 0;

		// If we are not a ragdoll and we have a valid pose, use the pose to
		// drive rope by setting a target position.
		if (m_pSkeletonAnim->m_IsAnimPlaying && !m_bPhysicsRelinquished)
		{
			pr.bTargetPoseActive = 1;
			for (int i=0; i<m_auxPhys[j].nBones; ++i)
			{
				pr.bTargetPoseActive =
					pr.bTargetPoseActive &&
					poseData.m_pJointsStatus[m_auxPhys[j].pauxBoneInfo[i].iBone].o != 0;
			}
		}

		pe_status_rope sr;
		if (peType == PE_SOFT)
		{
			pe_status_softvtx ssv;
			sr.pPoints = m_auxPhys[j].pVtx;
			if (m_auxPhys[j].pPhysEnt->GetStatus(&ssv))
			{
				for (int i=0; i<=m_auxPhys[j].nBones; ++i)
					sr.pPoints[i] = ssv.pVtx[i];

				sr.bTargetPoseActive = pr.bTargetPoseActive;
				CryBonePhysics &physInfo = GetModelJointPointer(m_auxPhys[j].iBoneTiedTo[0])->getPhysInfo(nLod);
				sr.bTargetPoseActive = sr.bTargetPoseActive && physInfo.min[0]>0.0f;
			}
			sr.nVtx = 0;
		}
		else
		{
			sr.pPoints = NULL;
			sr.pVtx = NULL;

			if (!m_auxPhys[j].pPhysEnt->GetStatus(&sr))
				continue;

			if (m_auxPhys[j].iBoneStiffnessController>=0)
			{
				Quat q = poseData.m_pJointsRelative[m_auxPhys[j].iBoneStiffnessController].q;
				if (m_pPhysBuffer)
					q = m_pPhysBuffer[m_auxPhys[j].iBoneStiffnessController].location.q;

				float angle = RAD2DEG(atan2_tpl(q.v.len(),q.w))*2.0f;
				angle -= 360*isneg(180.0f-angle);

				if (angle<0.01f && angle>-90.0f)
					angle = 0.01f;
				else if (angle>=100.0f || angle<-90.0f)
					angle = 0.0f;

				CryBonePhysics &physInfo = GetModelJointPointer(m_auxPhys[j].iBoneTiedTo[0])->getPhysInfo(nLod);
				pr.stiffnessAnim = RAD2DEG(physInfo.max[0])*angle*0.01f;
				pr.dampingAnim = RAD2DEG(physInfo.max[2])*angle*0.01f;
			}

			if (m_pCharPhysics)	
			{
				if (sr.bTargetPoseActive != 2)
					m_auxPhys[j].pPhysEnt->SetParams(&pr);
			}
/*	
			if (sr.nVtx)
			{
				if (sr.nVtx>m_auxPhys[j].nSubVtxAlloc)
				{
					if (m_auxPhys[j].pSubVtx)
						delete[] m_auxPhys[j].pSubVtx;

					m_auxPhys[j].pSubVtx = new Vec3[m_auxPhys[j].nSubVtxAlloc = (sr.nVtx-1&~15)+16];
				}

				sr.pVtx = m_auxPhys[j].pSubVtx;
				m_auxPhys[j].pPhysEnt->GetStatus(&sr);
				sr.pPoints = m_auxPhys[j].pVtx;

				float len = 0.0f;
				for (int i=0; i<sr.nVtx-1; ++i) 
					len += (sr.pVtx[i+1]-sr.pVtx[i]).GetLength();

				f32 rnSegs = 1.0f / m_auxPhys[j].nBones;
				int i;
				int iter = 3;
				do
				{
					sr.pPoints[0] = sr.pVtx[0];
					int ivtx;
					for (i=ivtx=0; i<m_auxPhys[j].nBones && ivtx<sr.nVtx-1;)
					{
						Vec3 dir = sr.pVtx[ivtx+1]-sr.pVtx[ivtx];
						Vec3 v0 = sr.pVtx[ivtx]-sr.pPoints[i];
						f32 ka = dir.len2();
						f32 kb = v0 * dir;
						f32 kc = v0.len2() - sqr(len * rnSegs);
						f32 kd = sqrt_tpl(max(0.0f, kb*kb - ka*kc));
						if (kd-kb<ka)
							sr.pPoints[++i] = sr.pVtx[ivtx]+dir*((kd-kb)/ka);
						else
							++ivtx;
					}
					len *= (1.0f - (m_auxPhys[j].nBones-i) * rnSegs);
					len += (sr.pVtx[sr.nVtx-1] - sr.pPoints[i]).GetLength();
				} while(--iter);

				sr.pPoints[m_auxPhys[j].nBones] = sr.pVtx[sr.nVtx-1];
				if (i+1<m_auxPhys[j].nBones)
				{
					Vec3 dir = sr.pPoints[m_auxPhys[j].nBones] - sr.pPoints[i];
					rnSegs = 1.0f / (m_auxPhys[j].nBones-i);

					for (int ivtx = i+1; ivtx < m_auxPhys[j].nBones; ++ivtx)
						sr.pPoints[ivtx] = sr.pPoints[i] + dir * ((ivtx-i) * rnSegs);
				}
			}
			else
			{
				sr.pPoints = m_auxPhys[j].pVtx;
				m_auxPhys[j].pPhysEnt->GetStatus(&sr);
			}
*/
			if (m_auxPhys[j].pVtx)
			{
				sr.pPoints = m_auxPhys[j].pVtx;
				m_auxPhys[j].pPhysEnt->GetStatus(&sr);
			}

		}

		atv.points = sr.pPoints;
		for (int i=0; i<m_auxPhys[j].nBones; ++i) 
		{
			Vec3* pPosition = &poseData.m_pJointsAbsolute[m_auxPhys[j].pauxBoneInfo[i].iBone].t;
			if (m_pPhysBuffer && m_bPhysBufferFilled)
				pPosition = &m_pPhysBuffer[m_auxPhys[j].pauxBoneInfo[i].iBone].location.t;

			assert( atv.points != NULL );
			atv.points[i] = qtRoot.GetInverted() * *pPosition;
		}
		int iParent = m_auxPhys[j].pauxBoneInfo[m_auxPhys[j].nBones-1].iBone;
		if (GetModelJointPointer(iParent)->numChildren() > 0)
		{
			int childIndex = GetModelJointChildIndex(iParent, 0);
			Vec3* pPosition = &poseData.m_pJointsAbsolute[childIndex].t;
			if (m_pPhysBuffer && m_bPhysBufferFilled)
				pPosition = &m_pPhysBuffer[childIndex].location.t;

			assert( atv.points != NULL );
			atv.points[m_auxPhys[j].nBones] = qtRoot.GetInverted() * *pPosition;
		}	
		if (nNeff && sr.bTargetPoseActive == 1)
		{
			atv.nPoints = m_auxPhys[j].nBones+1;
			m_auxPhys[j].pPhysEnt->Action(&atv);
		}

		pe_status_awake statusTmp;
		bRopesAwake |= m_auxPhys[j].pPhysEnt->GetStatus(&statusTmp);
	}
}

void CSkeletonPose::ProcessPhysicsImpact(Skeleton::CPoseData& poseData, float fDeltaTimePhys, int nNeff)
{
	pe_params_flags pf;
	pe_status_joint sj;
	pe_params_joint pj;
	pj.bNoUpdate = 1;
	if (fDeltaTimePhys > 0)
	{
		pj.animationTimeStep = fDeltaTimePhys;
		pj.ranimationTimeStep = 1.0f / fDeltaTimePhys;
	}

	int nLod = m_bPhysicsRelinquished ? GetPhysicsLod() : 0;

	int i, j, iParent;
	pe_status_awake statusTmp;
	if (m_bPhysicsAwake = (m_pCharPhysics->GetStatus(&statusTmp)!=0))
	{
		pf.flagsOR = pef_disabled;
		m_pCharPhysics->SetParams(&pf);
		Matrix33 mtxid;	mtxid.SetIdentity();
		int32 numJoints = GetJointCount();

		// read angles deltas from physics and set current angles from animation as qext to physics
		for(i=0; i<numJoints; i++)
		{
			if (GetModelJointPointer(i)->getPhysInfo(nLod).pPhysGeom && (iParent=getBonePhysParentIndex(i,nLod)) >= 0)
			{
				sj.idChildBody = i;
				if (!m_pCharPhysics->GetStatus(&sj))
					continue;

				Vec3 trans34 = poseData.m_pJointsRelative[i].t;
				Quat matparent; matparent.SetIdentity();

				for (int nCurParent=getBoneParentIndex(i); nCurParent != iParent; nCurParent=getBoneParentIndex(nCurParent)) 
					matparent = poseData.m_pJointsRelative[nCurParent].q * matparent;

				Quat mtx0;
				if(GetModelJointPointer(i)->getPhysInfo(nLod).flags!=-1 && GetModelJointPointer(i)->getPhysInfo(nLod).framemtx[0][0]<10)
				{					
					mtx0 = Quat( *alias_cast<Matrix33*>((float*)&GetModelJointPointer(i)->getPhysInfo(nLod).framemtx[0]) );
				}
				else
					mtx0 = Quat(IDENTITY);

				Quat matparent1;
				matparent1.SetIdentity();

				int iNextParent=iParent;
				int bUpdateMtx0 = 0;
				while (
					(GetModelJointPointer(iParent)->getPhysInfo(nLod).flags & all_angles_locked) == all_angles_locked &&
					(j = getBonePhysParentIndex(iParent, nLod)) >= 0)
				{
					iParent = j;
				}

				matparent1.SetIdentity();
				for (; iNextParent != iParent; iNextParent = getBoneParentIndex(iNextParent))
				{
					matparent1 = poseData.m_pJointsRelative[iNextParent].q * matparent1;
					bUpdateMtx0 = 1;
				}

				Quat mtx33 = !mtx0*matparent * poseData.m_pJointsRelative[i].q;
				Ang3 qextPhys = sj.qext;
				if (nNeff)
					sj.qext = Ang3::GetAnglesXYZ(mtx33);

				pj.op[0] = iParent;
				pj.op[1] = i;
				if (!m_bPhysicsRelinquished) 
				{
					pj.qext = sj.qext;
					Quat m33 = Quat::CreateRotationXYZ(sj.q + sj.qext);
					poseData.m_pJointsRelative[i].q = !matparent * mtx0 * m33;
					poseData.m_pJointsRelative[i].t = trans34;
					pj.pivot = poseData.m_pJointsAbsolute[iParent].GetInverted() * poseData.m_pJointsAbsolute[i].t;
					pj.flagsPivot = 8|1;
					if (bUpdateMtx0)
						pj.q0 = matparent1*mtx0;
					else
						MARK_UNUSED pj.q0;
				}
				else
				{
					pj.qtarget = sj.qext-qextPhys;
				}

				if (nNeff)
					m_pCharPhysics->SetParams(&pj);
			}
		}

		MARK_UNUSED pj.q0;
		if (!m_bPhysicsRelinquished)
			UnconvertBoneGlobalFromRelativeForm(poseData, false, nLod);

		int bStateChanged = 0;
		for(i=m_pInstance->m_AttachmentManager.GetAttachmentCount()-1; i>=0; i--) 
		{
			int icode = m_pInstance->m_AttachmentManager.UpdatePhysAttachmentHideState(i,m_pCharPhysics);
			bStateChanged += (icode^icode>>1)&1; // icode: 1 and 2 mean state chaged, 0 and 3 - stayed the same
		}
		if (bStateChanged)
		{
			for(i=0;i<m_nAuxPhys;i++)
			{
				if (!m_auxPhys[i].bTied0)
					continue;

				pe_params_rope pr; 
				pr.pEntTiedTo[0] = m_pCharPhysics;
				pr.idPartTiedTo[0] = getBonePhysParentIndex(m_auxPhys[i].iBoneTiedTo[0], nLod);
				m_auxPhys[i].pPhysEnt->SetParams(&pr);
			}
		}
	}

	if (!is_unused(pf.flagsOR) && pf.flagsOR)
	{
		pf.flagsOR = 0;
		pf.flagsAND = ~pef_disabled;
		m_pCharPhysics->SetParams(&pf);
	}
}

void CSkeletonPose::ProcessPhysics(Skeleton::CPoseData& poseData, const QuatT& rPhysEntityLocation, float fDeltaTimePhys, int nNeff, SCharUpdateFeedback *pCharFeedback, const QuatT& attachmentParent)
{
	if (Console::GetInst().ca_UsePhysics == 0)
		return;

	m_bPhysicsAwake = 0;
	m_vOffset = Vec3(0, 0, 0);

	if (m_pCharPhysics)
	{
		if (m_pCharPhysics->GetType() == PE_ARTICULATED)
		{
			Physics_SynchronizeToEntityArticulated(fDeltaTimePhys, nNeff);

			if (Console::GetInst().ca_physicsProcessImpact)
				ProcessPhysicsImpact(poseData, fDeltaTimePhys, nNeff);
		}
		else
		{
			if (m_pCharPhysics) 
				Physics_SynchronizeToEntity(*m_pCharPhysics, QuatT(IDENTITY));
		}
	}
	Physics_SynchronizeToAux(poseData, rPhysEntityLocation, nNeff, attachmentParent);

	m_bPhysicsWasAwake = m_bPhysicsAwake;

	ProcessPhysicsFallAndPlay(poseData, fDeltaTimePhys);
}


// Synch

void CSkeletonPose::SynchronizeWithPhysicsPrepare(Memory::CPool& memoryPool, const QuatT& locationPhysics, const QuatTS& locationAnimation)
{
	DEFINE_PROFILER_FUNCTION();

	SPU_SynchronizeWithPhysicsPrepare(memoryPool);
}

void CSkeletonPose::SynchronizeWithPhysicsPost(const QuatT& locationPhysics, const QuatTS& locationAnimation)
{
	DEFINE_PROFILER_FUNCTION();

	uint32 objectType =	m_pInstance->m_pModel->m_ObjectType;
	IPhysicalEntity* pCharPhys = GetCharacterPhysics();
	pe_params_flags pf1;
	if (objectType==CGA || pCharPhys && pCharPhys->GetType()==PE_ARTICULATED && pCharPhys->GetParams(&pf1) && pf1.flags & aef_recorded_physics)
	{

		if (m_bFullSkeletonUpdate && m_pSkeletonAnim->m_IsAnimPlaying || m_ppBonePhysics)
			//	if (m_InstanceVisible)
			m_pInstance->UpdatePhysicsCGA(GetPoseDataExplicitWriteable(), 1, locationAnimation );

		if (m_bFullSkeletonUpdate)
		{
			QuatT Offset = QuatT(locationPhysics.GetInverted() * locationAnimation);
			int32 numAttachments = m_pInstance->m_AttachmentManager.GetAttachmentCount()-1;

			for(int32 i=numAttachments-1; i>=0; i--) 
				m_pInstance->m_AttachmentManager.UpdatePhysicalizedAttachment(i,pCharPhys, Offset );

		}
	}	
}

void CSkeletonPose::SynchronizeWithPhysics(Skeleton::CPoseData& poseData, const QuatT& locationPhysics, const QuatTS& locationAnimation, const QuatT& attachmentParent)
{
	DEFINE_PROFILER_FUNCTION();

	uint32 objectType =	m_pInstance->m_pModel->m_ObjectType;
	if (objectType != CHR)
		return;

	pe_params_flags pf;
	if (m_bFullSkeletonUpdate)
	{
		if (gEnv->pPhysicalWorld->GetPhysVars()->lastTimeStep > 0 || m_pSkeletonAnim->GetCharEditMode())
		{
			ProcessPhysics(poseData, locationPhysics, m_pInstance->m_fOriginalDeltaTime, m_pSkeletonAnim->m_IsAnimPlaying, 0, attachmentParent);
			pf.flagsAND = ~pef_disabled;
			m_pInstance->SetWasVisible(1);
		}
		else
		{
			m_pInstance->SetWasVisible(0);
		}
	} 
	else
	{
		if (m_pCharPhysics || m_nAuxPhys>1)
			pf.flagsOR = pef_disabled;
	}

	m_pInstance->SetWasVisible(m_pInstance->GetWasVisible() & (((int)m_bFullSkeletonUpdate) > 0));
	SetAuxParams(&pf);
}

// SynchronizeTo

SBatchUpdateValidator SBatchUpdateValidator::g_firstValidator;
volatile int SBatchUpdateValidator::g_lockList;

void CSkeletonPose::Physics_SynchronizeToEntity(IPhysicalEntity& physicalEntity, QuatT offset)
{
	DEFINE_PROFILER_FUNCTION();

	IPhysicalEntity* pent = &physicalEntity;

	const Skeleton::CPoseData& poseData = GetPoseData();

	if (pent->GetType() == PE_ARTICULATED) 
	{
		for (uint32 i=0; i<(unsigned)m_nPhysJoints; ++i)
			m_physJoints[i] = poseData.m_pJointsAbsolute[m_physJointsIdx[i]];

		offset.t = -poseData.m_pJointsAbsolute[getBonePhysChildIndex(0)].t;

		pe_action_batch_parts_update abpu;
		abpu.numParts = m_nPhysJoints;
		abpu.pnumParts = &m_nPhysJoints;
		abpu.posParts = strided_pointer<Vec3>(&m_physJoints[0].t, sizeof(m_physJoints[0]));
		abpu.qParts = strided_pointer<Quat>(&m_physJoints[0].q, sizeof(m_physJoints[0]));
		abpu.posOffs = offset.t;
		abpu.qOffs = offset.q;
		abpu.pIds = &m_physJointsIdx[0];
		if (!m_pPhysUpdateValidator)
		{
			ScopedSwitchToGlobalHeap globalHeap;
			m_pPhysUpdateValidator = new SBatchUpdateValidator;
		}
		m_pPhysUpdateValidator->AddRef();
		abpu.pValidator = m_pPhysUpdateValidator;
		pent->Action(&abpu);
	}
	else
	{
		uint32 jointCount = GetJointCount();
		uint32 j=0;
		for (uint32 i=0; i<jointCount; ++i) 
		{
			if (GetModelJointPointer(i)->m_PhysInfo[0].pPhysGeom)
				j=i;
		}

		pe_params_part partpos;
		partpos.invTimeStep = 1.0f/max(0.001f,m_pInstance->m_fOriginalDeltaTime);
		for (uint32 i=0; i<jointCount; ++i) 
		{
			if (GetModelJointPointer(i)->m_PhysInfo[0].pPhysGeom)
			{
				partpos.partid = i;
				partpos.bRecalcBBox = i==j;
				QuatT q = offset*poseData.m_pJointsAbsolute[i];
				partpos.pos = q.t;
				partpos.q = q.q;
				pent->SetParams(&partpos);
			}
		}
	}

	int bStateChanged = 0;
	for(int i=m_pInstance->m_AttachmentManager.GetAttachmentCount()-1; i>=0; i--)
		bStateChanged |= m_pInstance->m_AttachmentManager.UpdatePhysicalizedAttachment(i, pent, offset);
	if (bStateChanged)
	{
		for(uint32 i=0; i<(unsigned)m_nAuxPhys; ++i)
		{
			if (!m_auxPhys[i].bTied0)
				continue;

			pe_params_rope pr; 
			pr.pEntTiedTo[0] = m_pCharPhysics;
			pr.idPartTiedTo[0] = getBonePhysParentIndex(m_auxPhys[i].iBoneTiedTo[0], 0);
			m_auxPhys[i].pPhysEnt->SetParams(&pr);
		}
	}
}

void CSkeletonPose::Physics_SynchronizeToEntityArticulated(float fDeltaTimePhys, int nNeff)
{
	if (!m_bPhysicsRelinquished)
	{
		const Skeleton::CPoseData& poseData = GetPoseData();

		//this is the relative orientation & translation of the animated character for this frame
		QuatT KinematicMovement = IDENTITY;

		pe_params_joint pj;
		pj.bNoUpdate = 1;
		if (fDeltaTimePhys > 0)
		{
			// IZF: These might not be needed.
			pj.animationTimeStep = fDeltaTimePhys;
			pj.ranimationTimeStep = 1.0f / fDeltaTimePhys;
		}

		MARK_UNUSED pj.pivot;
		int iRoot = getBonePhysChildIndex(0);
		if (m_bPhysicsAwake) 
		{	
			pj.q=Ang3(ZERO);
			pj.qext = Ang3::GetAnglesXYZ(Matrix33(poseData.m_pJointsAbsolute[iRoot].q));
			pj.op[0] = -1;
			pj.op[1] = iRoot;
			m_pCharPhysics->SetParams(&pj);
		}
		else
		{
			if (m_pCharPhysics)
				Physics_SynchronizeToEntity(*m_pCharPhysics, KinematicMovement);
		}

		pe_params_articulated_body pab;
		pab.pivot.zero();
		pab.posHostPivot = KinematicMovement.t + poseData.m_pJointsAbsolute[iRoot].t;//*m_fScale;
		pab.qHostPivot = KinematicMovement.q;
		pab.bRecalcJoints = m_bPhysicsAwake;
		m_velPivot = (pab.posHostPivot-m_prevPosPivot)/max(0.001f,m_pInstance->m_fOriginalDeltaTime);
		m_velPivot *= (float)isneg(m_velPivot.len2()-sqr(30.0f));
		m_prevPosPivot = pab.posHostPivot;
		m_pCharPhysics->SetParams(&pab);
	}
}

// FallAndPlay

void CSkeletonPose::FindSpineBones()
{
	if (m_nSpineBones)
		return;

	uint32 jointCount = GetJointCount();
	for (uint32 i=m_nSpineBones=0; i<jointCount && m_nSpineBones<3; ++i)
	{
		if (strstr(GetModelJointPointer(i)->GetJointName(), "Pelvis") ||
			strstr(GetModelJointPointer(i)->GetJointName(), "Spine"))
		{
			m_iSpineBone[m_nSpineBones++] = i;
		}
	}

	if (!m_nSpineBones)
	{
		m_iSpineBone[0] = getBonePhysChildIndex(0);
		m_nSpineBones = 1 + (m_iSpineBone[0] >> 31);
	}
}

void CSkeletonPose::Fall()
{
	m_bAliveRagdoll=true;
}

void CSkeletonPose::GoLimp()
{
	if (m_pCharPhysics && !m_bLimpRagdoll)
	{
		if (m_pCharPhysics)
		{
			pe_params_joint pj;
			int i,iParent;
			for(i=0; i<(int)GetJointCount(); i++)
			{
				if (GetModelJointPointer(i)->getPhysInfo(0).pPhysGeom && (iParent=getBonePhysParentIndex(i)) >= 0)
				{
					pj.op[0] = iParent;
					pj.op[1] = i;
					pj.ks.zero();
					m_pCharPhysics->SetParams(&pj);
				}
			}
		}

		m_bLimpRagdoll = true;
	}
}

bool CSkeletonPose::SetFnPAnimGroup(const char *name) 
{ 
	int i;
	for(i=m_pInstance->m_pModel->m_AnimationSet.m_arrStandupAnimTypes.size()-1; 
			i>=0 && strcmp(m_pInstance->m_pModel->m_AnimationSet.m_arrStandupAnimTypes[i],name); i--);
	if (i<0)
		return false; 
	m_iFnPSet=i; return true;
}
bool CSkeletonPose::SetFnPAnimGroup(int idx) 
{ 
	if (idx>=m_pInstance->m_pModel->m_AnimationSet.m_arrStandupAnimTypes.size())
		return false;
	m_iFnPSet=idx; return true; 
}

float CSkeletonPose::Falling() const
{
	return m_timeRagdolled;
}

float CSkeletonPose::Lying() const
{
	return m_timeLying;
}

int CSkeletonPose::GetFallingDir()
{
	if (!m_pCharPhysics)
		return -1;

	FindSpineBones();

	const Skeleton::CPoseData& poseData = GetPoseData();
	const QuatT* pJointsAbsolute = poseData.GetJointsAbsolute();

	pe_status_dynamics sd;
	int status = m_pCharPhysics->GetStatus(&sd);

	Vec3 n(ZERO);
	for(uint32 i=0; i<m_nSpineBones; ++i)
		n += sd.v * pJointsAbsolute[m_iSpineBone[i]].q;

	Vec3 nAbs = n.abs();
	int i = idxmax3((f32*)&nAbs);
	i = i*2 + isneg(n[i]);
	return i;
}

ILINE void ForceAnimationLoading(const CCharacterModel& model, int32 animationId)
{
	const CAnimationSet& animationSet = model.m_AnimationSet;
	const ModelAnimationHeader* pModelAnimation = animationSet.GetModelAnimationHeader(animationId);
	if (!pModelAnimation)
		return;
	if (pModelAnimation->m_nAssetType != CAF_File)
		return;

	uint32 rootCrc32 = model.m_ModelSkeleton.m_arrModelJoints[0].m_nJointCRC32;
	const int32 globalId = int32(pModelAnimation->m_nGlobalAnimId);
	GlobalAnimationHeaderCAF& animation = g_pCharacterManager->GetAnimationManager().m_arrGlobalCAF[globalId];
	if (animation.IsAssetOnDemand() && !animation.IsAssetLoaded())
		animation.LoadCAF(rootCrc32);
}

ILINE void InitializePoseAbsolute(const CModelSkeleton& skeleton, QuatT* pResult)
{
	const QuatT* pJointsAbsolute = skeleton.GetPoseData().GetJointsAbsolute();
	uint32 jointCount = skeleton.GetPoseData().GetJointCount();
	for (uint32 i=0; i<jointCount; ++i)
		pResult[i] = pJointsAbsolute[i];
}

ILINE void SamplePoseAbsolute(const CModelSkeleton& skeleton, GlobalAnimationHeaderCAF& animation, f32 t, QuatT* pResult)
{
	const Skeleton::CPoseData& poseData = skeleton.GetPoseData();
	const QuatT* pJointsRelativeDefault = poseData.GetJointsRelative();
	uint32 jointCount = poseData.GetJointCount();
	for (uint32 i=0; i<jointCount; ++i)
	{
		QuatT& result = pResult[i];
		result = pJointsRelativeDefault[i];
		const CModelJoint& joint = skeleton.m_arrModelJoints[i];
		IController* pController = animation.GetControllerByJointCRC32(joint.m_nJointCRC32);
		if (pController)
			pController->GetOP(0.0f, result.q, result.t);

		// Compute absolute value
		if (joint.m_idxParent >= 0)
			result = pResult[joint.m_idxParent] * result;
	}
}

ILINE bool SamplePoseAbsolute(const CCharacterModel& model, int32 animationId, f32 t, QuatT* pResult)
{
	const CAnimationSet& animationSet = model.m_AnimationSet;

	const ModelAnimationHeader* pModelAnimation = animationSet.GetModelAnimationHeader(animationId);
	if (!pModelAnimation)
	{
		InitializePoseAbsolute(model.m_ModelSkeleton, pResult);
		return false;
	}

	if (pModelAnimation->m_nAssetType != CAF_File)
	{
		InitializePoseAbsolute(model.m_ModelSkeleton, pResult);
		return false;
	}

	const int32 globalId = int32(pModelAnimation->m_nGlobalAnimId);
	if (globalId < 0)
	{
		InitializePoseAbsolute(model.m_ModelSkeleton, pResult);
		return false;
	}

	GlobalAnimationHeaderCAF& animation = g_pCharacterManager->GetAnimationManager().m_arrGlobalCAF[globalId];
	SamplePoseAbsolute(model.m_ModelSkeleton, animation, t, pResult);
	return true;
}

f32 ComputePoseDifference(
	const CCharacterModel& model,
	const QuatT* pJointsAbsolute0,
	const QuatT* pJointsAbsolute1,
	f32 scale,
	uint32 jointStart,
	uint32 jointCount,
	bool b3dof,
	int iLod)
{
	QuatT qDelta;
	if (!b3dof)
	{
		qDelta.SetRotationXYZ(Ang3(
			0.0f,
			0.0f,
			Ang3::GetAnglesXYZ(pJointsAbsolute0[jointStart].q).z -
			Ang3::GetAnglesXYZ(pJointsAbsolute1[jointStart].q).z));
	}
	else
	{
		qDelta.q = pJointsAbsolute0[jointStart].q * !pJointsAbsolute1[jointStart].q;
	}

	qDelta.SetTranslation(
		pJointsAbsolute0[jointStart].t - qDelta.q * pJointsAbsolute1[jointStart].t * scale);

	primitives::box bbox;
	f32 result = 0.0f;
	for (uint32 i=jointStart; i<jointCount; i++)
	{
		if (phys_geometry *pgeom = model.m_ModelSkeleton.m_arrModelJoints[i].m_PhysInfo[iLod].pPhysGeom)
		{
			pgeom->pGeom->GetBBox(&bbox);
			int iax = idxmax3((float*)&bbox.size);
			for (int j=-1; j<=1; j+=2)
		{
				Vec3 pt = bbox.center + bbox.Basis.GetRow(iax) * (bbox.size[iax] * j);
				result += (pJointsAbsolute0[i] * pt - qDelta * (pJointsAbsolute1[i] * pt * scale)).len2() * pgeom->V;
			}
		}
		}

	return result;
}

ILINE int32 FindMatchingPose(
	const CCharacterModel& model,
	const Skeleton::CPoseData& poseData,
	f32 scale,
	uint32 jointStart,
	const int32* pAnimationIds,
	uint32 animationCount,
	bool b3dof,
	int iLod)
{
	uint32 jointCount = poseData.GetJointCount();
	QuatT joints[256];

	f32 differenceMin = 1e10f;
	int32 differenceMinAnimationId = -1;
	for (uint32 i=0; i<animationCount; ++i)
	{
		int32 animationId = pAnimationIds[i];

		ForceAnimationLoading(model, animationId);
		SamplePoseAbsolute(model, animationId, 0.0f, joints);

		f32 difference = ComputePoseDifference(
			model, poseData.GetJointsAbsolute(), joints, scale,
			jointStart, jointCount, b3dof, iLod);
		if (difference < differenceMin)
		{
			differenceMin = difference;
			differenceMinAnimationId = animationId;
		}
	}

	return differenceMinAnimationId;
}

void CSkeletonPose::StandUp(const Matrix34 &_mtx, bool b3DOF, IPhysicalEntity *&pNewPhysicalEntity, Matrix34 &mtxDelta)
{
	if (!m_bPhysicsRelinquished)
		return;

	if (!m_pInstance->m_pModel)
		return;
	CCharacterModel& model = *m_pInstance->m_pModel;

	Skeleton::CPoseData& poseDataWriteable = GetPoseDataForceWriteable();
	uint32 jointCount = poseDataWriteable.GetJointCount();

	if (m_arrPhysicsJoints.empty())
		InitPhysicsSkeleton();
	FindSpineBones();

	m_bPhysicsWasAwake = 1;
	m_b3DOFStandup = b3DOF;

	//

	int32 animationId = -1;
	if (m_nSpineBones)
	{
		uint32 animationCount = uint32(model.m_AnimationSet.m_arrStandupAnims.size());
		const DynArray<int>& animationIds = m_iFnPSet < int32(animationCount) ?
			model.m_AnimationSet.m_arrStandupAnims[m_iFnPSet] : DynArray<int>();
		animationCount = uint32(animationIds.size());

		if (animationCount > 0)
		{
			animationId = FindMatchingPose(
				model, poseDataWriteable, m_fScale, m_iSpineBone[0],
				&animationIds[0], animationCount, m_b3DOFStandup != 0, GetPhysicsLod());
		}
	}

	QuatT joints[256];
	SamplePoseAbsolute(model, animationId, 0.0f, joints);

	//

	if (m_pPrevCharHost)
		g_pIPhysicalWorld->DestroyPhysicalEntity(m_pPrevCharHost,2);

	Matrix34 mtxChar = Matrix34(Vec3(m_fScale),Quat(IDENTITY),m_vOffset);
	CreateCharacterPhysics(m_pPrevCharHost, m_fMass, m_iSurfaceIdx, m_stiffnessScale, 0, mtxChar);
	CreateAuxilaryPhysics(m_pCharPhysics, mtxChar);
	
	// rotate the entity around z by current (physical) z angle - animation z angle
	Quat drotRoot;
	if (!b3DOF)
	{
		drotRoot.SetRotationXYZ(Ang3(
			0.0f,
			0.0f,
		(Ang3::GetAnglesXYZ(poseDataWriteable.m_pJointsAbsolute[m_iSpineBone[0]].q)).z - 
			(Ang3::GetAnglesXYZ(joints[m_iSpineBone[0]].q)).z));
	}
	else
	{
		drotRoot = poseDataWriteable.m_pJointsAbsolute[m_iSpineBone[0]].q *	!joints[m_iSpineBone[0]].q;
	}

	Matrix34 mtx = _mtx;
	mtx.Scale(Vec3(1.0f / _mtx.GetColumn(0).len()));
	mtxDelta = Matrix33(drotRoot);
	mtxDelta.SetTranslation(mtx*poseDataWriteable.m_pJointsAbsolute[m_iSpineBone[0]].t - 
		mtxDelta*joints[m_iSpineBone[0]].t*m_fScale);
	// find the closest orientation in each animated bone to the current pos

	// start playing the stand-up anim (store the original q diff, 1/blend-away-time per bone)
	CryCharAnimationParams aparamns;
	aparamns.m_nFlags &= ~CA_LOOP_ANIMATION;
	aparamns.m_nFlags |= CA_FORCE_SKELETON_UPDATE;
	m_pSkeletonAnim->StopAnimationsAllLayers();
	m_pSkeletonAnim->StartAnimationById(animationId, aparamns);
	m_pInstance->SetAnimationSpeed(1.0f);

	CAnimationSet *pAnimSet = (CAnimationSet*)m_pInstance->GetIAnimationSet();
	QuatT mtxDeltaInv = QuatT(mtxDelta).GetInverted(), mtxQ=QuatT(mtx);

	uint32 numJoints = GetJointCount();
	m_arrPhysicsJoints[0].m_qRelFallPlay = poseDataWriteable.m_pJointsAbsolute[0].q;
	for (uint32 j=1; j<numJoints; j++)
	{
		m_arrPhysicsJoints[j].m_qRelFallPlay =
			!poseDataWriteable.m_pJointsAbsolute[m_parrModelJoints[j].m_idxParent].q * poseDataWriteable.m_pJointsAbsolute[j].q;
	}

	int32 sidx = m_iSpineBone[0];
	m_arrPhysicsJoints[sidx].m_qRelFallPlay =
		!drotRoot * poseDataWriteable.m_pJointsAbsolute[sidx].q;


	for (uint32 i=0; i<jointCount; i++)
	{
		poseDataWriteable.m_pJointsAbsolute[i] = mtxDeltaInv * (mtxQ * poseDataWriteable.m_pJointsAbsolute[i]);
	}

	if (m_pCharPhysics) 
	{
		pe_params_articulated_body pab;
		pab.bAwake = 0;
		pab.pivot.zero();
		pab.posHostPivot = m_vOffset+Vec3( joints[m_iSpineBone[0]].t );
		pab.qHostPivot.SetIdentity();
		pab.bRecalcJoints = 0;
		m_pCharPhysics->SetParams(&pab);
	}

	if (!b3DOF)
	{
		pe_player_dynamics pd; //pd.bActive = 0;
		pe_params_flags pf;	pf.flagsOR = pef_ignore_areas;
		pd.gravity.Set(0,0,-1.8f);
		if (m_pPrevCharHost)
			m_pPrevCharHost->SetParams(&pd),m_pPrevCharHost->SetParams(&pf);
	}
	m_timeStandingUp = 0;
	mtxDelta.Scale(Vec3(m_fScale));

	pNewPhysicalEntity = m_pPrevCharHost;

	// IZF: We have to force the recomputation of skinning transformations since
	// there might not be a further call to animation processing which would
	// lead to 1-frame with the wrong pose.
	m_pInstance->BeginSkinningTransformationsComputation();
}

void CSkeletonPose::ProcessPhysicsFallAndPlay(Skeleton::CPoseData& poseData, float fDeltaTimePhys)
{ 
	if (!m_pInstance->m_pModel)
		return;
	CCharacterModel& model = *m_pInstance->m_pModel;

	if (m_bPhysicsRelinquished && m_bAliveRagdoll && m_iFnPSet<(int)model.m_AnimationSet.m_arrStandupAnims.size())
	{ 
		// if char relinquished and stiff
		Quat drotRoot;
		pe_status_dynamics sd;
		m_pCharPhysics->GetStatus(&sd);
		m_timeRagdolled += fDeltaTimePhys;
/*
		if (sd.nContacts==0 && m_timeLying<=0.0f && m_timeNoColl<=0.0f)
		{	
			//   if he's flying - check the 'body' normal (pelvis+spines), play new fall anim if sextant changes
			m_pSkeletonAnim->StartAnimation("falling_head_nw_up_01", CryCharAnimationParams(0, CA_LOOP_ANIMATION));
		}
*/
		if (sd.nContacts>=1) 
		{	// if collisions present - pause the anim; if not for some time - resume the anim
			m_pInstance->SetAnimationSpeed(0);
			m_timeNoColl = 0;
		}	
		else
		{ 
			if ((m_timeNoColl+=fDeltaTimePhys)>0.1f)
				m_pInstance->SetAnimationSpeed(1.0f);
		}

		if (sd.nContacts>=2)
		{
			if (m_timeLying < 0.0f)
				m_timeLying = 0.0f;

			int32 animationId = -1;
			if (m_nSpineBones)
			{
				uint32 animationCount = uint32(model.m_AnimationSet.m_arrStandupAnims.size());
				const DynArray<int>& animationIds = m_iFnPSet < int32(animationCount) ?
					model.m_AnimationSet.m_arrStandupAnims[m_iFnPSet] : DynArray<int>();
				animationCount = uint32(animationIds.size());

				animationId = FindMatchingPose(
					model, poseData, m_fScale, m_iSpineBone[0],
					&animationIds[0], animationCount, m_b3DOFStandup != 0, GetPhysicsLod());
				}

			CryCharAnimationParams params;
			params.m_fPlaybackSpeed = 0.8f;
			params.m_nFlags |= CA_REPEAT_LAST_KEY;
			m_pSkeletonAnim->StopAnimationsAllLayers();
			m_pSkeletonAnim->StartAnimationById(animationId, params);
			m_pInstance->SetAnimationSpeed(0);

			m_timeLying += fDeltaTimePhys;
		}
	}

	if (m_timeStandingUp >= 0)
	{
		uint32 numJoints = GetJointCount();
		f32 t = min(1.0f, m_timeStandingUp);
		poseData.m_pJointsRelative[0].q.SetNlerp(m_arrPhysicsJoints[0].m_qRelFallPlay, poseData.m_pJointsRelative[0].q, t);
		poseData.m_pJointsAbsolute[0].q = poseData.m_pJointsRelative[0].q;

		for (uint32 j0=1; j0<numJoints; j0++)
		{
			poseData.m_pJointsRelative[j0].q.SetNlerp( m_arrPhysicsJoints[j0].m_qRelFallPlay,poseData.m_pJointsRelative[j0].q, t);
			poseData.m_pJointsAbsolute[j0] = poseData.m_pJointsAbsolute[m_parrModelJoints[j0].m_idxParent]*poseData.m_pJointsRelative[j0];
		}

		if (m_pCharPhysics)
			Physics_SynchronizeToEntity(*m_pCharPhysics, QuatT(IDENTITY));

		if ((m_timeStandingUp += fDeltaTimePhys) > 2.0f)
		{
			pe_params_articulated_body pab2;
			pe_player_dynamics pd; pd.bActive = 1;
			pe_params_flags pf2; pf2.flagsAND = ~pef_ignore_areas;
			if (!m_b3DOFStandup && m_pCharPhysics && m_pCharPhysics->GetParams(&pab2) && pab2.pHost)
				pab2.pHost->SetParams(&pd),pab2.pHost->SetParams(&pf2);
			m_timeStandingUp = -1.0f;
		}
	}
}
